diff options
Diffstat (limited to 'src/modules')
47 files changed, 2076 insertions, 2073 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 6068ab082..4c64c88ae 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -568,7 +568,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds R = R_decl * R_body; /* copy rotation matrix */ - memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); + memcpy(&att.R[0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; // compute secondary attitude @@ -595,7 +595,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitch_sec = euler_angles_sec(1); att.yaw_sec = euler_angles_sec(2); - memcpy(&att.R_sec[0][0], &R_adapted.data[0][0], sizeof(att.R_sec)); + memcpy(&att.R_sec[0], &R_adapted.data[0], sizeof(att.R_sec)); att.rollspeed_sec = -x_aposteriori[2]; att.pitchspeed_sec = x_aposteriori[1]; diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index e0bcbc6e9..53ab74305 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -59,6 +59,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/wind_estimate.h> #include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_global_position.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 6768bfa7e..f3cd87728 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -101,7 +101,7 @@ void Block::updateParams() void Block::updateSubscriptions() { - uORB::SubscriptionBase *sub = getSubscriptions().getHead(); + uORB::SubscriptionNode *sub = getSubscriptions().getHead(); int count = 0; while (sub != NULL) { @@ -119,7 +119,7 @@ void Block::updateSubscriptions() void Block::updatePublications() { - uORB::PublicationBase *pub = getPublications().getHead(); + uORB::PublicationNode *pub = getPublications().getHead(); int count = 0; while (pub != NULL) { diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 9bd80b15b..d2f9cdf07 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -46,8 +46,8 @@ // forward declaration namespace uORB { - class SubscriptionBase; - class PublicationBase; + class SubscriptionNode; + class PublicationNode; } namespace control @@ -83,15 +83,15 @@ public: protected: // accessors SuperBlock *getParent() { return _parent; } - List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; } - List<uORB::PublicationBase *> & getPublications() { return _publications; } + List<uORB::SubscriptionNode *> & getSubscriptions() { return _subscriptions; } + List<uORB::PublicationNode *> & getPublications() { return _publications; } List<BlockParamBase *> & getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; float _dt; - List<uORB::SubscriptionBase *> _subscriptions; - List<uORB::PublicationBase *> _publications; + List<uORB::SubscriptionNode *> _subscriptions; + List<uORB::PublicationNode *> _publications; List<BlockParamBase *> _params; private: diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index e8fecef0d..454d0db19 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -82,16 +82,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos, BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // subscriptions - _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), - _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), - _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), - _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20), - _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), - _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + _att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()), + _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()), + _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()), + _pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()), + _missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()), + _manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()), + _status(ORB_ID(vehicle_status), 20, &getSubscriptions()), + _param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz // publications - _actuators(&getPublications(), ORB_ID(actuator_controls_0)) + _actuators(ORB_ID(actuator_controls_0), &getPublications()) { } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e7805daa9..c0b1bb404 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -83,6 +83,7 @@ #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> +#include <platforms/px4_defines.h> #include "estimator_23states.h" @@ -1384,7 +1385,7 @@ FixedwingEstimator::task_main() math::Vector<3> euler = R.to_euler(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = R(i, j); + PX4_R(_att.R, i, j) = R(i, j); _att.timestamp = _last_sensor_timestamp; _att.q[0] = _ekf->states[0]; diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp new file mode 100644 index 000000000..f543c02f9 --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -0,0 +1,325 @@ +/* Copyright (c) 2014 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** + * @file mc_att_control_base.cpp + * + * @author Roman Bapst <bapstr@ethz.ch> + * + */ + +#include "fw_att_control_base.h" +#include <math.h> +#include <mathlib/mathlib.h> +#include <drivers/drv_hrt.h> + +using namespace std; + +FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : + + _task_should_exit(false), _task_running(false), _control_task(-1), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite output")), + /* states */ + _setpoint_valid(false), _debug(false) { + /* safely initialize structs */ + _att = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; + _vehicle_status = {}; + +} + +FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() { + +} + +void FixedwingAttitudeControlBase::control_attitude() { + bool lock_integrator = false; + static int loop_counter = 0; + /* scale around tuning airspeed */ + + float airspeed; + + /* if airspeed is not updating, we assume the normal average speed */ + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) + || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + airspeed = _parameters.airspeed_trim; + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } + } else { + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); + } + + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim + / ((airspeed < _parameters.airspeed_min) ? + _parameters.airspeed_min : airspeed); + + float roll_sp = _parameters.rollsp_offset_rad; + float pitch_sp = _parameters.pitchsp_offset_rad; + float throttle_sp = 0.0f; + + if (_vcontrol_mode.flag_control_velocity_enabled + || _vcontrol_mode.flag_control_position_enabled) { + /* read in attitude setpoint from attitude setpoint uorb topic */ + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } + } else { + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do around 45 degrees, the mapping is + * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. + * + * The trim gets subtracted here from the manual setpoint to get + * the intended attitude setpoint. Later, after the rate control step the + * trim is added again to the control signal. + */ + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max + - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + throttle_sp = _manual.z; + _actuators.control[4] = _manual.flaps; + + /* + * in manual mode no external source should / does emit attitude setpoints. + * emit the manual setpoint here to allow attitude controller tuning + * in attitude control mode. + */ + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = roll_sp; + att_sp.pitch_body = pitch_sp; + att_sp.yaw_body = 0.0f - _parameters.trim_yaw; + att_sp.thrust = throttle_sp; + + } + + /* If the aircraft is on ground reset the integrators */ + if (_vehicle_status.condition_landed) { + _roll_ctrl.reset_integrator(); + _pitch_ctrl.reset_integrator(); + _yaw_ctrl.reset_integrator(); + } + + /* Prepare speed_body_u and speed_body_w */ + float speed_body_u = 0.0f; + float speed_body_v = 0.0f; + float speed_body_w = 0.0f; + if (_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vel_n + + _att.R[1][0] * _global_pos.vel_e + + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + + _att.R[1][1] * _global_pos.vel_e + + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + + _att.R[1][2] * _global_pos.vel_e + + _att.R[2][2] * _global_pos.vel_d; + } else { + if (_debug && loop_counter % 10 == 0) { + warnx("Did not get a valid R\n"); + } + } + + /* Run attitude controllers */ + if (isfinite(roll_sp) && isfinite(pitch_sp)) { + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, + speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), + _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, + _att.yawspeed, _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[0] = + (isfinite(roll_u)) ? + roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!isfinite(roll_u)) { + _roll_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + + if (_debug && loop_counter % 10 == 0) { + warnx("roll_u %.4f", (double) roll_u); + } + } + + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[1] = + (isfinite(pitch_u)) ? + pitch_u + _parameters.trim_pitch : + _parameters.trim_pitch; + if (!isfinite(pitch_u)) { + _pitch_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", (double) pitch_u, + (double) _yaw_ctrl.get_desired_rate(), + (double) airspeed, (double) airspeed_scaling, + (double) roll_sp, (double) pitch_sp, + (double) _roll_ctrl.get_desired_rate(), + (double) _pitch_ctrl.get_desired_rate(), + (double) _att_sp.roll_body); + } + } + + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[2] = + (isfinite(yaw_u)) ? + yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + if (!isfinite(yaw_u)) { + _yaw_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("yaw_u %.4f", (double) yaw_u); + } + } + + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + if (!isfinite(throttle_sp)) { + if (_debug && loop_counter % 10 == 0) { + warnx("throttle_sp %.4f", (double) throttle_sp); + } + } + } else { + perf_count(_nonfinite_input_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", + (double) roll_sp, (double) pitch_sp); + } + } + +} + +void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) { + // watch out, still need to see where we modify attitude for the tailsitter case + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _att.q[0] = quat(0); + _att.q[1] = quat(1); + _att.q[2] = quat(2); + _att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _att.R[0][0] = Rot(0,0); + _att.R[1][0] = Rot(1,0); + _att.R[2][0] = Rot(2,0); + _att.R[0][1] = Rot(0,1); + _att.R[1][1] = Rot(1,1); + _att.R[2][1] = Rot(2,1); + _att.R[0][2] = Rot(0,2); + _att.R[1][2] = Rot(1,2); + _att.R[2][2] = Rot(2,2); + + _att.R_valid = true; +} +void FixedwingAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + _att.rollspeed = angular_rate(0); + _att.pitchspeed = angular_rate(1); + _att.yawspeed = angular_rate(2); +} +void FixedwingAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _att_sp.roll_body = control_attitude_thrust_reference(0); + _att_sp.pitch_body = control_attitude_thrust_reference(1); + _att_sp.yaw_body = control_attitude_thrust_reference(2); + _att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); + _att_sp.R_body[0][0] = Rot_sp(0,0); + _att_sp.R_body[1][0] = Rot_sp(1,0); + _att_sp.R_body[2][0] = Rot_sp(2,0); + _att_sp.R_body[0][1] = Rot_sp(0,1); + _att_sp.R_body[1][1] = Rot_sp(1,1); + _att_sp.R_body[2][1] = Rot_sp(2,1); + _att_sp.R_body[0][2] = Rot_sp(0,2); + _att_sp.R_body[1][2] = Rot_sp(1,2); + _att_sp.R_body[2][2] = Rot_sp(2,2); +} +void FixedwingAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h new file mode 100644 index 000000000..bde1b755f --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -0,0 +1,151 @@ +#ifndef FW_ATT_CONTROL_BASE_H_ +#define FW_ATT_CONTROL_BASE_H_ + +/* Copyright (c) 2014 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** + * @file fw_att_control_base.h + * + * @author Roman Bapst <bapstr@ethz.ch> + * + */ + +#include <ecl/attitude_fw/ecl_pitch_controller.h> +#include <ecl/attitude_fw/ecl_roll_controller.h> +#include <ecl/attitude_fw/ecl_yaw_controller.h> + +#include <uORB/topics/airspeed.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_status.h> + +#include <systemlib/perf_counter.h> + +class FixedwingAttitudeControlBase +{ +public: + /** + * Constructor + */ + FixedwingAttitudeControlBase(); + + /** + * Destructor + */ + ~FixedwingAttitudeControlBase(); + + +protected: + + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _control_task; /**< task handle for sensor task */ + + 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 airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ + + struct { + float tconst; + float p_p; + float p_d; + float p_i; + float p_ff; + float p_rmax_pos; + float p_rmax_neg; + float p_integrator_max; + float p_roll_feedforward; + float r_p; + float r_d; + float r_i; + float r_ff; + float r_integrator_max; + float r_rmax; + float y_p; + float y_i; + float y_d; + float y_ff; + float y_roll_feedforward; + float y_integrator_max; + float y_coordinated_min_speed; + float y_rmax; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + + float trim_roll; + float trim_pitch; + float trim_yaw; + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ + + } _parameters; /**< local copies of interesting parameters */ + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + + void control_attitude(); + + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion<double> attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + +}; + +#endif /* FW_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8d18ae68c..0025aae7f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -76,6 +76,7 @@ #include <ecl/attitude_fw/ecl_pitch_controller.h> #include <ecl/attitude_fw/ecl_roll_controller.h> #include <ecl/attitude_fw/ecl_yaw_controller.h> +#include <platforms/px4_defines.h> /** * Fixedwing attitude control app start / stop handling function @@ -743,15 +744,15 @@ FixedwingAttitudeControl::task_main() _att.roll = euler_angles(0); _att.pitch = euler_angles(1); _att.yaw = euler_angles(2); - _att.R[0][0] = R_adapted(0, 0); - _att.R[0][1] = R_adapted(0, 1); - _att.R[0][2] = R_adapted(0, 2); - _att.R[1][0] = R_adapted(1, 0); - _att.R[1][1] = R_adapted(1, 1); - _att.R[1][2] = R_adapted(1, 2); - _att.R[2][0] = R_adapted(2, 0); - _att.R[2][1] = R_adapted(2, 1); - _att.R[2][2] = R_adapted(2, 2); + PX4_R(_att.R, 0, 0) = R_adapted(0, 0); + PX4_R(_att.R, 0, 1) = R_adapted(0, 1); + PX4_R(_att.R, 0, 2) = R_adapted(0, 2); + PX4_R(_att.R, 1, 0) = R_adapted(1, 0); + PX4_R(_att.R, 1, 1) = R_adapted(1, 1); + PX4_R(_att.R, 1, 2) = R_adapted(1, 2); + PX4_R(_att.R, 2, 0) = R_adapted(2, 0); + PX4_R(_att.R, 2, 1) = R_adapted(2, 1); + PX4_R(_att.R, 2, 2) = R_adapted(2, 2); // lastly, roll- and yawspeed have to be swaped float helper = _att.rollspeed; @@ -849,7 +850,7 @@ FixedwingAttitudeControl::task_main() _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { - /* + /* * Velocity should be controlled and manual is enabled. */ roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) @@ -922,9 +923,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; - speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; - speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; + speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d; + speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d; + speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d; } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index dbf15d98a..74249c9c5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -77,6 +77,7 @@ #include <uORB/topics/navigation_capabilities.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_status.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/pid/pid.h> @@ -90,6 +91,7 @@ #include <external_lgpl/tecs/tecs.h> #include "landingslope.h" #include "mtecs/mTecs.h" +#include <platforms/px4_defines.h> static int _control_task = -1; /**< task handle for sensor task */ @@ -735,7 +737,7 @@ FixedwingPositionControl::vehicle_attitude_poll() /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _R_nb(i, j) = _att.R[i][j]; + _R_nb(i, j) = PX4_R(_att.R, i, j); } } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index bffeefc1f..ecdab2936 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -52,7 +52,7 @@ mTecs::mTecs() : _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ - _status(&getPublications(), ORB_ID(tecs_status)), + _status(ORB_ID(tecs_status), &getPublications()), /* control blocks */ _controlTotalEnergy(this, "THR"), _controlEnergyDistribution(this, "PIT", true), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe..cb55a25aa 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -778,7 +778,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp new file mode 100644 index 000000000..90a6c90fe --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -0,0 +1,306 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control.cpp + * Multicopter attitude controller. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> + */ + +#include "mc_att_control.h" +#include "mc_att_control_params.h" +#include "math.h" + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +namespace mc_att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +} + +MulticopterAttitudeControl::MulticopterAttitudeControl() : + MulticopterAttitudeControlBase(), + _task_should_exit(false), + _actuators_0_circuit_breaker_enabled(false), + + /* publications */ + _att_sp_pub(nullptr), + _v_rates_sp_pub(nullptr), + _actuators_0_pub(nullptr), + _n(), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + +{ + _params_handles.roll_p = PX4_PARAM_INIT(MC_ROLL_P); + _params_handles.roll_rate_p = PX4_PARAM_INIT(MC_ROLLRATE_P); + _params_handles.roll_rate_i = PX4_PARAM_INIT(MC_ROLLRATE_I); + _params_handles.roll_rate_d = PX4_PARAM_INIT(MC_ROLLRATE_D); + _params_handles.pitch_p = PX4_PARAM_INIT(MC_PITCH_P); + _params_handles.pitch_rate_p = PX4_PARAM_INIT(MC_PITCHRATE_P); + _params_handles.pitch_rate_i = PX4_PARAM_INIT(MC_PITCHRATE_I); + _params_handles.pitch_rate_d = PX4_PARAM_INIT(MC_PITCHRATE_D); + _params_handles.yaw_p = PX4_PARAM_INIT(MC_YAW_P); + _params_handles.yaw_rate_p = PX4_PARAM_INIT(MC_YAWRATE_P); + _params_handles.yaw_rate_i = PX4_PARAM_INIT(MC_YAWRATE_I); + _params_handles.yaw_rate_d = PX4_PARAM_INIT(MC_YAWRATE_D); + _params_handles.yaw_ff = PX4_PARAM_INIT(MC_YAW_FF); + _params_handles.yaw_rate_max = PX4_PARAM_INIT(MC_YAWRATE_MAX); + _params_handles.man_roll_max = PX4_PARAM_INIT(MC_MAN_R_MAX); + _params_handles.man_pitch_max = PX4_PARAM_INIT(MC_MAN_P_MAX); + _params_handles.man_yaw_max = PX4_PARAM_INIT(MC_MAN_Y_MAX); + _params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX); + _params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX); + _params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX); + _params_handles.autostart_id = PX4_PARAM_INIT(SYS_AUTOSTART); + + /* fetch initial parameter values */ + parameters_update(); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ + _is_vtol = true; + } + else { + _is_vtol = false; + } + + /* + * do subscriptions + */ + _v_att = PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + _v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0); + _v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0); + _v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0); + _parameter_update = PX4_SUBSCRIBE(_n, parameter_update, + MulticopterAttitudeControl::handle_parameter_update, this, 1000); + _manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0); + _armed = PX4_SUBSCRIBE(_n, actuator_armed, 0); + _v_status = PX4_SUBSCRIBE(_n, vehicle_status, 0); + +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ +} + +int +MulticopterAttitudeControl::parameters_update() +{ + float v; + + /* roll gains */ + PX4_PARAM_GET(_params_handles.roll_p, &v); + _params.att_p(0) = v; + PX4_PARAM_GET(_params_handles.roll_rate_p, &v); + _params.rate_p(0) = v; + PX4_PARAM_GET(_params_handles.roll_rate_i, &v); + _params.rate_i(0) = v; + PX4_PARAM_GET(_params_handles.roll_rate_d, &v); + _params.rate_d(0) = v; + + /* pitch gains */ + PX4_PARAM_GET(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + PX4_PARAM_GET(_params_handles.pitch_rate_p, &v); + _params.rate_p(1) = v; + PX4_PARAM_GET(_params_handles.pitch_rate_i, &v); + _params.rate_i(1) = v; + PX4_PARAM_GET(_params_handles.pitch_rate_d, &v); + _params.rate_d(1) = v; + + /* yaw gains */ + PX4_PARAM_GET(_params_handles.yaw_p, &v); + _params.att_p(2) = v; + PX4_PARAM_GET(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; + PX4_PARAM_GET(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; + PX4_PARAM_GET(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; + + PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff); + PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + _params.yaw_rate_max = math::radians(_params.yaw_rate_max); + + /* manual control scale */ + PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max); + PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max); + PX4_PARAM_GET(_params_handles.man_yaw_max, &_params.man_yaw_max); + _params.man_roll_max = math::radians(_params.man_roll_max); + _params.man_pitch_max = math::radians(_params.man_pitch_max); + _params.man_yaw_max = math::radians(_params.man_yaw_max); + + /* acro control scale */ + PX4_PARAM_GET(_params_handles.acro_roll_max, &v); + _params.acro_rate_max(0) = math::radians(v); + PX4_PARAM_GET(_params_handles.acro_pitch_max, &v); + _params.acro_rate_max(1) = math::radians(v); + PX4_PARAM_GET(_params_handles.acro_yaw_max, &v); + _params.acro_rate_max(2) = math::radians(v); + + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + + return OK; +} + +void MulticopterAttitudeControl::handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg) +{ + parameters_update(); +} + +void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { + + perf_begin(_loop_perf); + + /* run controller on attitude changes */ + static uint64_t last_run = 0; + float dt = (px4::get_time_micros() - last_run) / 1000000.0f; + last_run = px4::get_time_micros(); + + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; + + } else if (dt > 0.02f) { + dt = 0.02f; + } + + if (_v_control_mode->get().flag_control_attitude_enabled) { + control_attitude(dt); + + /* publish the attitude setpoint if needed */ + if (_publish_att_sp && _v_status->get().is_rotary_wing) { + _v_att_sp_mod.timestamp = px4::get_time_micros(); + + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_v_att_sp_mod); + + } else { + _att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint); + } + } + + /* publish attitude rates setpoint */ + _v_rates_sp_mod.roll = _rates_sp(0); + _v_rates_sp_mod.pitch = _rates_sp(1); + _v_rates_sp_mod.yaw = _rates_sp(2); + _v_rates_sp_mod.thrust = _thrust_sp; + _v_rates_sp_mod.timestamp = px4::get_time_micros(); + + if (_v_rates_sp_pub != nullptr) { + _v_rates_sp_pub->publish(_v_rates_sp_mod); + } else { + if (_is_vtol) { + _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); + } else { + _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + } + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_control_mode->get().flag_control_manual_enabled) { + /* manual rates control - ACRO mode */ + _rates_sp = math::Vector<3>(_manual_control_sp->get().y, -_manual_control_sp->get().x, + _manual_control_sp->get().r).emult(_params.acro_rate_max); + _thrust_sp = _manual_control_sp->get().z; + + /* reset yaw setpoint after ACRO */ + _reset_yaw_sp = true; + + /* publish attitude rates setpoint */ + _v_rates_sp_mod.roll = _rates_sp(0); + _v_rates_sp_mod.pitch = _rates_sp(1); + _v_rates_sp_mod.yaw = _rates_sp(2); + _v_rates_sp_mod.thrust = _thrust_sp; + _v_rates_sp_mod.timestamp = px4::get_time_micros(); + + if (_v_rates_sp_pub != nullptr) { + _v_rates_sp_pub->publish(_v_rates_sp_mod); + + } else { + if (_is_vtol) { + _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); + } else { + _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + } + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + _rates_sp(0) = _v_rates_sp->get().roll; + _rates_sp(1) = _v_rates_sp->get().pitch; + _rates_sp(2) = _v_rates_sp->get().yaw; + _thrust_sp = _v_rates_sp->get().thrust; + } + } + + if (_v_control_mode->get().flag_control_rates_enabled) { + control_attitude_rates(dt); + + /* 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 = px4::get_time_micros(); + + if (!_actuators_0_circuit_breaker_enabled) { + if (_actuators_0_pub != nullptr) { + _actuators_0_pub->publish(_actuators); + + } else { + if (_is_vtol) { + _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc); + } else { + _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); + } + } + } + } +} diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h new file mode 100644 index 000000000..33552c269 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control.h @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control.h + * Multicopter attitude controller. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. + */ + +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <systemlib/perf_counter.h> +// #include <systemlib/systemlib.h> +#include <systemlib/circuit_breaker.h> +#include <lib/mathlib/mathlib.h> + +#include "mc_att_control_base.h" + +class MulticopterAttitudeControl : + public MulticopterAttitudeControlBase +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControl(); + + /* Callbacks for topics */ + void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); + void handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg); + + void spin() { _n.spin(); } + +private: + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + + px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */ + px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */ + px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */ + + bool _is_vtol; /**< true if vehicle is vtol, to be replaced with global API */ + + px4::NodeHandle _n; + + struct { + px4_param_t roll_p; + px4_param_t roll_rate_p; + px4_param_t roll_rate_i; + px4_param_t roll_rate_d; + px4_param_t pitch_p; + px4_param_t pitch_rate_p; + px4_param_t pitch_rate_i; + px4_param_t pitch_rate_d; + px4_param_t yaw_p; + px4_param_t yaw_rate_p; + px4_param_t yaw_rate_i; + px4_param_t yaw_rate_d; + px4_param_t yaw_ff; + px4_param_t yaw_rate_max; + + px4_param_t man_roll_max; + px4_param_t man_pitch_max; + px4_param_t man_yaw_max; + px4_param_t acro_roll_max; + px4_param_t acro_pitch_max; + px4_param_t acro_yaw_max; + + px4_param_t autostart_id; + } _params_handles; /**< handles for interesting parameters */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /** + * Update our local parameter cache. + */ + int parameters_update(); +}; + diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp new file mode 100644 index 000000000..d7fbf3acd --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -0,0 +1,309 @@ +/* Copyright (c) 2014 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** + * @file mc_att_control_base.cpp + * + * MC Attitude Controller : Control and math code + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> + * + */ + +#include "mc_att_control_base.h" +#include <geo/geo.h> +#include <math.h> +#include <lib/mathlib/mathlib.h> + +#ifdef CONFIG_ARCH_ARM +#else +#include <cmath> +using namespace std; +#endif + +MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _publish_att_sp(false) + +{ + memset(&_v_rates_sp, 0, sizeof(_v_att_sp_mod)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp_mod)); + memset(&_actuators, 0, sizeof(_actuators)); + + _params.att_p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; + _params.acro_rate_max.zero(); + + _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + _I.identity(); +} + +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() +{ +} + +void MulticopterAttitudeControlBase::control_attitude(float dt) +{ + float yaw_sp_move_rate = 0.0f; + _publish_att_sp = false; + + + if (_v_control_mode->get().flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode->get().flag_control_velocity_enabled + || _v_control_mode->get().flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + _v_att_sp_mod = _v_att_sp->get(); + } + + if (!_v_control_mode->get().flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp_mod.thrust = _manual_control_sp->get().z; + _publish_att_sp = true; + } + + if (!_armed->get().armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp_mod.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max; + _v_att_sp_mod.yaw_body = _wrap_pi( + _v_att_sp_mod.yaw_body + yaw_sp_move_rate * dt); + float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); + float yaw_offs = _wrap_pi(_v_att_sp_mod.yaw_body - _v_att->get().yaw); + + if (yaw_offs < -yaw_offs_max) { + _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max); + + } else if (yaw_offs > yaw_offs_max) { + _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max); + } + + _v_att_sp_mod.R_valid = false; + _publish_att_sp = true; + } + + /* reset yaw setpint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp_mod.yaw_body = _v_att->get().yaw; + _v_att_sp_mod.R_valid = false; + _publish_att_sp = true; + } + + if (!_v_control_mode->get().flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp_mod.roll_body = _manual_control_sp->get().y * _params.man_roll_max; + _v_att_sp_mod.pitch_body = -_manual_control_sp->get().x + * _params.man_pitch_max; + _v_att_sp_mod.R_valid = false; + _publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + _v_att_sp_mod = _v_att_sp->get(); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp_mod.thrust; + + /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + + if (_v_att_sp_mod.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_v_att_sp_mod.R_body[0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_v_att_sp_mod.roll_body, _v_att_sp_mod.pitch_body, + _v_att_sp_mod.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp_mod.R_body[0], &R_sp.data[0][0], + sizeof(_v_att_sp_mod.R_body)); + _v_att_sp_mod.R_valid = true; + } + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.set(_v_att->get().R); + + /* all input data is ready, run controller itself */ + + /* 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)); + + /* axis and sin(angle) of desired rotation */ + math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + 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; + + e_R = e_R_z_axis * e_R_z_angle; + + /* 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); + + /* 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 { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* 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; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); + + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, + _params.yaw_rate_max); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} + +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) +{ + /* reset integral if disarmed */ + if (!_armed->get().armed || !_v_status->get().is_rotary_wing) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector < 3 > rates; + rates(0) = _v_att->get().rollspeed; + rates(1) = _v_att->get().pitchspeed; + rates(2) = _v_att->get().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 > MIN_TAKEOFF_THRUST) { + 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; + } + } + } + } + +} + +void MulticopterAttitudeControlBase::set_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; +} diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h new file mode 100644 index 000000000..cf4c726a7 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -0,0 +1,134 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* Copyright (c) 2014 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** + * @file mc_att_control_base.h + * + * MC Attitude Controller : Control and math code + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> + * + */ +#include <px4.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <errno.h> +#include <math.h> + +#include <systemlib/perf_counter.h> +#include <lib/mathlib/mathlib.h> + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlBase +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlBase(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlBase(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + void control_attitude(float dt); + void control_attitude_rates(float dt); + + void set_actuator_controls(); + +protected: + px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att; /**< vehicle attitude */ + px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp; /**< vehicle attitude setpoint */ + px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp; /**< vehicle rates setpoint */ + px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */ + px4::PX4_SUBSCRIBER(parameter_update) *_parameter_update; /**< parameter update */ + px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */ + px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */ + px4::PX4_SUBSCRIBER(vehicle_status) *_v_status; /**< vehicle status */ + + PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp_mod; /**< modified vehicle attitude setpoint + that gets published eventually */ + PX4_TOPIC_T(vehicle_rates_setpoint) _v_rates_sp_mod; /**< vehicle rates setpoint + that gets published eventually*/ + PX4_TOPIC_T(actuator_controls) _actuators; /**< actuator controls */ + + 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 { + 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 yaw_rate_max; /**< max yaw rate */ + + float man_roll_max; + float man_pitch_max; + float man_yaw_max; + math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + + int32_t autostart_id; + } _params; + + bool _publish_att_sp; + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ 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 0702e6378..4188c7880 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -38,6 +38,9 @@ * @author Tobias Naegeli <naegelit@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> * * The controller has two loops: P loop for angular error and PD loop for angular rate error. * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. @@ -49,933 +52,91 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> #include <string.h> -#include <unistd.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_control_mode.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/actuator_armed.h> -#include <uORB/topics/parameter_update.h> -#include <systemlib/param/param.h> -#include <systemlib/err.h> -#include <systemlib/perf_counter.h> -#include <systemlib/systemlib.h> -#include <systemlib/circuit_breaker.h> -#include <lib/mathlib/mathlib.h> -#include <lib/geo/geo.h> +#include <cstdlib> +#include "mc_att_control.h" -/** - * Multicopter attitude control app start / stop handling function - * - * @ingroup apps - */ -extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); - -#define YAW_DEADZONE 0.05f -#define MIN_TAKEOFF_THRUST 0.2f -#define RATES_I_LIMIT 0.3f - -class MulticopterAttitudeControl -{ -public: - /** - * Constructor - */ - MulticopterAttitudeControl(); - - /** - * Destructor, also kills the main task - */ - ~MulticopterAttitudeControl(); - - /** - * Start the multicopter attitude control task. - * - * @return OK on success. - */ - int start(); - -private: - - bool _task_should_exit; /**< if true, task_main() should exit */ - int _control_task; /**< task handle */ - - 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 */ - int _vehicle_status_sub; /**< vehicle status 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_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 - - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - - 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 */ - struct vehicle_status_s _vehicle_status; /**< vehicle status */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - 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 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 yaw_rate_p; - param_t yaw_rate_i; - param_t yaw_rate_d; - param_t yaw_ff; - param_t yaw_rate_max; - - param_t man_roll_max; - param_t man_pitch_max; - param_t man_yaw_max; - param_t acro_roll_max; - param_t acro_pitch_max; - param_t acro_yaw_max; - - param_t autostart_id; //what frame are we using? - } _params_handles; /**< handles for interesting parameters */ - - struct { - 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 yaw_rate_max; /**< max yaw rate */ - - float man_roll_max; - float man_pitch_max; - float man_yaw_max; - math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - - param_t autostart_id; - } _params; - - /** - * Update our local parameter cache. - */ - int parameters_update(); - - /** - * Check for parameter update and handle it. - */ - void parameter_update_poll(); - - /** - * Check for changes in vehicle control mode. - */ - void vehicle_control_mode_poll(); - - /** - * Check for changes in manual inputs. - */ - void vehicle_manual_poll(); - - /** - * Check for attitude setpoint updates. - */ - void vehicle_attitude_setpoint_poll(); - - /** - * Check for rates setpoint updates. - */ - void vehicle_rates_setpoint_poll(); - - /** - * Check for arming status updates. - */ - void arming_status_poll(); - - /** - * Attitude controller. - */ - void control_attitude(float dt); - - /** - * Attitude rates controller. - */ - void control_attitude_rates(float dt); - - /** - * Check for vehicle status updates. - */ - void vehicle_status_poll(); - - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main attitude control task. - */ - void task_main(); -}; - -namespace mc_att_control -{ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -MulticopterAttitudeControl *g_control; -} - -MulticopterAttitudeControl::MulticopterAttitudeControl() : - - _task_should_exit(false), - _control_task(-1), - -/* subscriptions */ - _v_att_sub(-1), - _v_att_sp_sub(-1), - _v_control_mode_sub(-1), - _params_sub(-1), - _manual_control_sp_sub(-1), - _armed_sub(-1), - _vehicle_status_sub(-1), - -/* publications */ - _att_sp_pub(-1), - _v_rates_sp_pub(-1), - _actuators_0_pub(-1), - - _actuators_0_circuit_breaker_enabled(false), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) - -{ - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_actuators, 0, sizeof(_actuators)); - memset(&_armed, 0, sizeof(_armed)); - memset(&_vehicle_status, 0, sizeof(_vehicle_status)); - _vehicle_status.is_rotary_wing = true; - - _params.att_p.zero(); - _params.rate_p.zero(); - _params.rate_i.zero(); - _params.rate_d.zero(); - _params.yaw_ff = 0.0f; - _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; - _params.acro_rate_max.zero(); - - _params.autostart_id = 0; //default - - _rates_prev.zero(); - _rates_sp.zero(); - _rates_int.zero(); - _thrust_sp = 0.0f; - _att_control.zero(); - - _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.yaw_rate_max = param_find("MC_YAWRATE_MAX"); - _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); - _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); - _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); - _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); - _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); - _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); - - _params_handles.autostart_id = param_find("SYS_AUTOSTART"); - - /* fetch initial parameter values */ - parameters_update(); - // set correct uORB ID, depending on if vehicle is VTOL or not - if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ - _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_virtual_mc); - } - else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_0); - } -} - -MulticopterAttitudeControl::~MulticopterAttitudeControl() +static bool thread_running = false; /**< Deamon status flag */ +static int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 { - if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_control_task); - break; - } - } while (_control_task != -1); - } - - mc_att_control::g_control = nullptr; +bool task_should_exit = false; } -int -MulticopterAttitudeControl::parameters_update() -{ - float v; - - /* roll gains */ - 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 gains */ - 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 gains */ - 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.yaw_rate_i, &v); - _params.rate_i(2) = 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.yaw_rate_max, &_params.yaw_rate_max); - _params.yaw_rate_max = math::radians(_params.yaw_rate_max); - - /* manual control scale */ - param_get(_params_handles.man_roll_max, &_params.man_roll_max); - param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); - param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); - _params.man_roll_max = math::radians(_params.man_roll_max); - _params.man_pitch_max = math::radians(_params.man_pitch_max); - _params.man_yaw_max = math::radians(_params.man_yaw_max); - - /* acro control scale */ - param_get(_params_handles.acro_roll_max, &v); - _params.acro_rate_max(0) = math::radians(v); - param_get(_params_handles.acro_pitch_max, &v); - _params.acro_rate_max(1) = math::radians(v); - param_get(_params_handles.acro_yaw_max, &v); - _params.acro_rate_max(2) = math::radians(v); - - _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); +using namespace px4; - param_get(_params_handles.autostart_id, &_params.autostart_id); - - return OK; -} - -void -MulticopterAttitudeControl::parameter_update_poll() -{ - bool updated; - - /* Check if parameters have changed */ - 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(); - } +PX4_MAIN_FUNCTION(mc_att_control); +void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp); } -void -MulticopterAttitudeControl::vehicle_control_mode_poll() -{ - bool updated; - - /* Check if vehicle control mode has changed */ - orb_check(_v_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); - } -} - -void -MulticopterAttitudeControl::vehicle_manual_poll() -{ - bool updated; - - /* get pilots inputs */ - orb_check(_manual_control_sp_sub, &updated); - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); - } -} - -void -MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_att_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); - } -} - -void -MulticopterAttitudeControl::vehicle_rates_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_rates_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); - } -} - -void -MulticopterAttitudeControl::arming_status_poll() -{ - /* 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); - } -} - -void -MulticopterAttitudeControl::vehicle_status_poll() -{ - /* check if there is new status information */ - bool vehicle_status_updated; - orb_check(_vehicle_status_sub, &vehicle_status_updated); - - if (vehicle_status_updated) { - orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); - } -} - -/* - * 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) +#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps */ -void -MulticopterAttitudeControl::control_attitude(float dt) -{ - float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; - - if (_v_control_mode.flag_control_manual_enabled) { - /* manual input, set or modify attitude setpoint */ - 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(); - } - - 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.z; - publish_att_sp = true; - } - - if (!_armed.armed) { - /* reset yaw setpoint when disarmed */ - _reset_yaw_sp = true; - } - - /* 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 { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; - _v_att_sp.yaw_body = _wrap_pi(_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 = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); - if (yaw_offs < - yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - 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.R_valid = false; - publish_att_sp = true; - } - - /* 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; - } - - 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.y * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - } else { - /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - vehicle_attitude_setpoint_poll(); - - /* reset yaw setpoint after non-manual control mode */ - _reset_yaw_sp = true; - } - - _thrust_sp = _v_att_sp.thrust; - - /* construct attitude setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - - 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]); - - } 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); - - /* 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; - } - - /* publish the attitude setpoint if needed */ - if (publish_att_sp && _vehicle_status.is_rotary_wing) { - _v_att_sp.timestamp = hrt_absolute_time(); - - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); - } - } - - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att.R); - - /* all input data is ready, run controller itself */ - - /* 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)); - - /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); - - /* calculate angle error */ - float e_R_z_sin = e_R.length(); - float e_R_z_cos = R_z * R_sp_z; - - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); - - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; - - 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; - - e_R = e_R_z_axis * e_R_z_angle; - - /* 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); - - /* 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 { - /* zero roll/pitch rotation */ - R_rp = R; - } - - /* 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; - } - - /* calculate angular rates setpoint */ - _rates_sp = _params.att_p.emult(e_R); - - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); - - /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; -} - -/* - * Attitude rates controller. - * Input: '_rates_sp' vector, '_thrust_sp' - * Output: '_att_control' vector - */ -void -MulticopterAttitudeControl::control_attitude_rates(float dt) +extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]) { - /* reset integral if disarmed */ - if (!_armed.armed || !_vehicle_status.is_rotary_wing) { - _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 > MIN_TAKEOFF_THRUST) { - 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; + px4::init(argc, argv, "mc_att_control"); - 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; - } - } - } + if (argc < 1) { + errx(1, "usage: mc_att_control {start|stop|status}"); } -} -void -MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) -{ - mc_att_control::g_control->task_main(); -} - -void -MulticopterAttitudeControl::task_main() -{ - - /* - * 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)); - _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* initialize parameters cache */ - parameters_update(); - - /* wakeup source: vehicle attitude */ - struct pollfd fds[1]; - - fds[0].fd = _v_att_sub; - fds[0].events = POLLIN; - - while (!_task_should_exit) { - - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) - continue; - - /* 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; - } - - perf_begin(_loop_perf); - - /* 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(); - - /* guard against too small (< 2ms) and too large (> 20ms) dt's */ - if (dt < 0.002f) { - dt = 0.002f; - - } else if (dt > 0.02f) { - dt = 0.02f; - } - - /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - - /* check for updates in other topics */ - parameter_update_poll(); - vehicle_control_mode_poll(); - arming_status_poll(); - vehicle_manual_poll(); - vehicle_status_poll(); - - if (_v_control_mode.flag_control_attitude_enabled) { - control_attitude(dt); - - /* 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 (_v_rates_sp_pub > 0) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - - } else { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled) { - /* 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); - _thrust_sp = _manual_control_sp.z; - - /* reset yaw setpoint after ACRO */ - _reset_yaw_sp = true; - - /* 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 (_v_rates_sp_pub > 0) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - - } else { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = _v_rates_sp.thrust; - } - } - - if (_v_control_mode.flag_control_rates_enabled) { - control_attitude_rates(dt); - - /* 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_circuit_breaker_enabled) { - if (_actuators_0_pub > 0) { - orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - - } else { - _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); - } + if (!strcmp(argv[1], "start")) { - } - } + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); } - perf_end(_loop_perf); - } - - warnx("exit"); - - _control_task = -1; - _exit(0); -} + task_should_exit = false; -int -MulticopterAttitudeControl::start() -{ - ASSERT(_control_task == -1); - - /* start the task */ - _control_task = task_spawn_cmd("mc_att_control", + daemon_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, - (main_t)&MulticopterAttitudeControl::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - warn("task start failed"); - return -errno; - } - - return OK; -} - -int mc_att_control_main(int argc, char *argv[]) -{ - if (argc < 1) - errx(1, "usage: mc_att_control {start|stop|status}"); - - if (!strcmp(argv[1], "start")) { - - if (mc_att_control::g_control != nullptr) - errx(1, "already running"); - - mc_att_control::g_control = new MulticopterAttitudeControl; - - if (mc_att_control::g_control == nullptr) - errx(1, "alloc failed"); - - if (OK != mc_att_control::g_control->start()) { - delete mc_att_control::g_control; - mc_att_control::g_control = nullptr; - err(1, "start failed"); - } + 3000, + mc_att_control_task_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } if (!strcmp(argv[1], "stop")) { - if (mc_att_control::g_control == nullptr) - errx(1, "not running"); - - delete mc_att_control::g_control; - mc_att_control::g_control = nullptr; + task_should_exit = true; exit(0); } if (!strcmp(argv[1], "status")) { - if (mc_att_control::g_control) { - errx(0, "running"); + if (thread_running) { + warnx("is running"); } else { - errx(1, "not running"); + warnx("not started"); } + + exit(0); } warnx("unrecognized command"); return 1; } +#endif + +PX4_MAIN_FUNCTION(mc_att_control) +{ + PX4_INFO("starting"); + MulticopterAttitudeControl attctl; + thread_running = true; + attctl.spin(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} + + 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 819847b40..a0b1706f2 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -40,6 +40,8 @@ * @author Anton Babushkin <anton.babushkin@me.com> */ +#include <px4_defines.h> +#include "mc_att_control_params.h" #include <systemlib/param/param.h> /** @@ -50,7 +52,7 @@ * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P); /** * Roll rate P gain @@ -60,7 +62,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P); /** * Roll rate I gain @@ -70,7 +72,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I); /** * Roll rate D gain @@ -80,7 +82,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D); /** * Pitch P gain @@ -91,7 +93,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P); /** * Pitch rate P gain @@ -101,7 +103,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P); /** * Pitch rate I gain @@ -111,7 +113,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I); /** * Pitch rate D gain @@ -121,7 +123,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D); /** * Yaw P gain @@ -132,7 +134,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PX4_PARAM_DEFINE_FLOAT(MC_YAW_P); /** * Yaw rate P gain @@ -142,7 +144,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P); /** * Yaw rate I gain @@ -152,7 +154,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I); /** * Yaw rate D gain @@ -162,7 +164,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D); /** * Yaw feed forward @@ -173,7 +175,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); * @max 1.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); +PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF); /** * Max yaw rate @@ -185,7 +187,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX); /** * Max manual roll @@ -195,7 +197,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); * @max 90.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f); +PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX); /** * Max manual pitch @@ -205,7 +207,7 @@ PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f); * @max 90.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); +PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX); /** * Max manual yaw rate @@ -214,7 +216,7 @@ PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); +PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX); /** * Max acro roll rate @@ -224,7 +226,7 @@ PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX); /** * Max acro pitch rate @@ -234,7 +236,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX); /** * Max acro yaw rate @@ -243,4 +245,4 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f); +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX); diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/mc_att_control/mc_att_control_params.h index 0f6c9aca1..59dd5240f 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/mc_att_control/mc_att_control_params.h @@ -1,6 +1,7 @@ + /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,38 +33,33 @@ ****************************************************************************/ /** - * @file actuator_armed.h - * - * Actuator armed topic + * @file mc_att_control_params.h + * Parameters for multicopter attitude controller. * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> */ +#pragma once -#ifndef TOPIC_ACTUATOR_ARMED_H -#define TOPIC_ACTUATOR_ARMED_H - -#include <stdint.h> -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - - uint64_t timestamp; /**< Microseconds since system boot */ - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ - bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(actuator_armed); - -#endif +#define PARAM_MC_ROLL_P_DEFAULT 6.0f +#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f +#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f +#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f +#define PARAM_MC_PITCH_P_DEFAULT 6.0f +#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f +#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f +#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f +#define PARAM_MC_YAW_P_DEFAULT 2.0f +#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f +#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f +#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f +#define PARAM_MC_YAW_FF_DEFAULT 0.5f +#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f +#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f +#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f +#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f +#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f +#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f +#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp new file mode 100644 index 000000000..d516d7e52 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -0,0 +1,142 @@ +/* Copyright (c) 2014 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** + * @file mc_att_control_sim.cpp + * + * MC Attitude Controller Interface for usage in a simulator + * + * @author Roman Bapst <bapstr@ethz.ch> + * + */ + +#include "mc_att_control_sim.h" +#include <geo/geo.h> +#include <math.h> + +#ifdef CONFIG_ARCH_ARM +#else +#include <cmath> +using namespace std; +#endif + +MulticopterAttitudeControlSim::MulticopterAttitudeControlSim() +{ + /* setup standard gains */ + //XXX: make these configurable + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; +} + +MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim() +{ +} + +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude) +{ + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3, 3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0, 0); + _v_att.R[1][0] = Rot(1, 0); + _v_att.R[2][0] = Rot(2, 0); + _v_att.R[0][1] = Rot(0, 1); + _v_att.R[1][1] = Rot(1, 1); + _v_att.R[2][1] = Rot(2, 1); + _v_att.R[0][2] = Rot(0, 2); + _v_att.R[1][2] = Rot(1, 2); + _v_att.R[2][2] = Rot(2, 2); + + _v_att.R_valid = true; +} + +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate) +{ + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); +} + +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.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30; + + // setup rotation matrix + math::Matrix<3, 3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0, 0); + _v_att_sp.R_body[1][0] = Rot_sp(1, 0); + _v_att_sp.R_body[2][0] = Rot_sp(2, 0); + _v_att_sp.R_body[0][1] = Rot_sp(0, 1); + _v_att_sp.R_body[1][1] = Rot_sp(1, 1); + _v_att_sp.R_body[2][1] = Rot_sp(2, 1); + _v_att_sp.R_body[0][2] = Rot_sp(0, 2); + _v_att_sp.R_body[1][2] = Rot_sp(1, 2); + _v_att_sp.R_body[2][2] = Rot_sp(2, 2); +} + +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs) +{ + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h new file mode 100644 index 000000000..a1bf44fc9 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -0,0 +1,97 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* Copyright (c) 2014 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** + * @file mc_att_control_sim.h + * + * MC Attitude Controller Interface for usage in a simulator + * + * @author Roman Bapst <bapstr@ethz.ch> + * + */ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <errno.h> +#include <math.h> +#include <drivers/drv_hrt.h> + +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/actuator_armed.h> +#include <systemlib/err.h> +#include <systemlib/perf_counter.h> +#include <lib/mathlib/mathlib.h> +#inlcude "mc_att_control_base.h" + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlSim : + public MulticopterAttitudeControlBase + +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlSim(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlSim(); + + /* setters and getters for interface with euroc-gazebo simulator */ + void set_attitude(const Eigen::Quaternion<double> attitude); + void set_attitude_rates(const Eigen::Vector3d &angular_rate); + void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d &motor_inputs); + +protected: + void vehicle_attitude_setpoint_poll() {}; + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk index 64b876f69..29254f5bb 100644 --- a/src/modules/mc_att_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -38,4 +38,6 @@ MODULE_COMMAND = mc_att_control SRCS = mc_att_control_main.cpp \ - mc_att_control_params.c + mc_att_control.cpp \ + mc_att_control_base.cpp \ + mc_att_control_params.c diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 3b631e2ce..bf65d2805 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -43,8 +43,9 @@ * @author Anton Babushkin <anton.babushkin@me.com> */ -#include <nuttx/config.h> -#include <stdio.h> +#include <px4.h> +#include <functional> +#include <cstdio> #include <stdlib.h> #include <string.h> #include <unistd.h> @@ -54,8 +55,7 @@ #include <poll.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> + #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> @@ -67,12 +67,13 @@ #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <systemlib/param/param.h> -#include <systemlib/err.h> +// #include <systemlib/param/param.h> +// #include <systemlib/err.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> #include <lib/geo/geo.h> #include <mavlink/mavlink_log.h> +#include <platforms/px4_defines.h> #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f @@ -531,9 +532,9 @@ MulticopterPositionControl::reset_pos_sp() if (_reset_pos_sp) { _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ - _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } @@ -987,7 +988,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; @@ -1148,11 +1149,11 @@ MulticopterPositionControl::task_main() /* thrust compensation for altitude only control mode */ float att_comp; - if (_att.R[2][2] > TILT_COS_MAX) { - att_comp = 1.0f / _att.R[2][2]; + if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / PX4_R(_att.R, 2, 2); - } else if (_att.R[2][2] > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + } else if (PX4_R(_att.R, 2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f; saturation_z = true; } else { @@ -1260,7 +1261,7 @@ MulticopterPositionControl::task_main() } /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ @@ -1275,7 +1276,7 @@ MulticopterPositionControl::task_main() R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 0af01cba1..4bdb4d539 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -68,6 +68,7 @@ #include <geo/geo.h> #include <systemlib/systemlib.h> #include <drivers/drv_hrt.h> +#include <platforms/px4_defines.h> #include "position_estimator_inav_params.h" #include "inertial_filter.h" @@ -282,13 +283,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; - + float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; - + float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; @@ -461,7 +462,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) acc[i] = 0.0f; for (int j = 0; j < 3; j++) { - acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } @@ -494,7 +495,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && - (att.R[2][2] > 0.7f) && + (PX4_R(att.R, 2, 2) > 0.7f) && (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { sonar_time = t; @@ -531,15 +532,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; - if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) { + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) { /* distance to surface */ - float flow_dist = dist_bottom / att.R[2][2]; + float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { - body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1]; + body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1]; } /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ @@ -562,7 +563,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* project measurements vector to NED basis, skip Z component */ for (int i = 0; i < 2; i++) { for (int j = 0; j < 3; j++) { - flow_v[i] += att.R[i][j] * flow_m[j]; + flow_v[i] += PX4_R(att.R, i, j) * flow_m[j]; } } @@ -571,7 +572,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_flow[1] = flow_v[1] - y_est[1]; /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); - w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); + w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy @@ -646,13 +647,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; - /* only reset the z estimate if the z weight parameter is not zero */ + /* only reset the z estimate if the z weight parameter is not zero */ if (params.w_z_vision_p > MIN_VALID_W) { z_est[0] = vision.z; z_est[1] = vision.vz; } - + vision_valid = true; warnx("VISION estimate valid"); mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); @@ -912,7 +913,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float c = 0.0f; for (int j = 0; j < 3; j++) { - c += att.R[j][i] * accel_bias_corr[j]; + c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { @@ -937,7 +938,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float c = 0.0f; for (int j = 0; j < 3; j++) { - c += att.R[j][i] * accel_bias_corr[j]; + c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cdcb428dd..fca72c304 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -182,13 +182,13 @@ private: /** * Get and limit value for specified RC function. Returns NAN if not mapped. */ - float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value); + float get_rc_value(uint8_t func, float min_value, float max_value); /** * Get switch position for specified function. */ - switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv); - switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv); + uint8_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); + uint8_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); /** * Gather and publish RC input data. @@ -771,25 +771,25 @@ Sensors::parameters_update() _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); /* update RC function mappings */ - _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; - _rc.function[ROLL] = _parameters.rc_map_roll - 1; - _rc.function[PITCH] = _parameters.rc_map_pitch - 1; - _rc.function[YAW] = _parameters.rc_map_yaw - 1; - - _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; - _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1; - _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; - _rc.function[ACRO] = _parameters.rc_map_acro_sw - 1; - _rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1; - - _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; - - _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; - _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; - _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; - _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; - _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; + _rc.function[RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; + _rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; + + _rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); @@ -1492,7 +1492,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } float -Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) +Sensors::get_rc_value(uint8_t func, float min_value, float max_value) { if (_rc.function[func] >= 0) { float value = _rc.channels[_rc.function[func]]; @@ -1512,8 +1512,8 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } } -switch_pos_t -Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv) +uint8_t +Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; @@ -1533,8 +1533,8 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo } } -switch_pos_t -Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv) +uint8_t +Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; @@ -1668,24 +1668,24 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value(ROLL, -1.0, 1.0); - manual.x = get_rc_value(PITCH, -1.0, 1.0); - manual.r = get_rc_value(YAW, -1.0, 1.0); - manual.z = get_rc_value(THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/systemlib/circuit_breaker.cpp index 68964deb0..ea478a60f 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,31 +31,25 @@ * ****************************************************************************/ -/** - * @file parameter_update.h - * Notification about a parameter update. - */ - -#ifndef TOPIC_PARAMETER_UPDATE_H -#define TOPIC_PARAMETER_UPDATE_H - -#include <stdint.h> -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ +/* + * @file circuit_breaker.c + * + * Circuit breaker parameters. + * Analog to real aviation circuit breakers these parameters + * allow to disable subsystems. They are not supported as standard + * operation procedure and are only provided for development purposes. + * To ensure they are not activated accidentally, the associated + * parameter needs to set to the key (magic). */ -struct parameter_update_s { - /** time at which the latest parameter was updated */ - uint64_t timestamp; -}; +#include <px4.h> +#include <systemlib/circuit_breaker.h> -/** - * @} - */ +bool circuit_breaker_enabled(const char* breaker, int32_t magic) +{ + int32_t val; + (void)PX4_PARAM_GET_BYNAME(breaker, &val); -ORB_DECLARE(parameter_update); + return (val == magic); +} -#endif
\ No newline at end of file diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index b3431722f..c97dbc26f 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -61,7 +61,7 @@ __BEGIN_DECLS -__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); +extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); __END_DECLS diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker_params.c index 12187d70e..e499ae27a 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -42,8 +42,8 @@ * parameter needs to set to the key (magic). */ -#include <systemlib/param/param.h> -#include <systemlib/circuit_breaker.h> +#include <px4.h> +#include <systemlib/circuit_breaker_params.h> /** * Circuit breaker for power supply check @@ -56,7 +56,7 @@ * @max 894281 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); +PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); /** * Circuit breaker for rate controller output @@ -69,7 +69,7 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); * @max 140253 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); +PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); /** * Circuit breaker for IO safety @@ -81,7 +81,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); * @max 22027 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); /** * Circuit breaker for airspeed sensor @@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); * @max 162128 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); +PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); /** * Circuit breaker for flight termination @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); +PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); /** * Circuit breaker for engine failure detection @@ -120,27 +120,4 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @max 284953 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); - -/** - * Circuit breaker for gps failure detection - * - * Setting this parameter to 240024 will disable the gps failure detection. - * If the aircraft is in gps failure mode the gps failure flag will be - * set to healthy - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 240024 - * @group Circuit Breaker - */ -PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); - -bool circuit_breaker_enabled(const char* breaker, int32_t magic) -{ - int32_t val; - (void)param_get(param_find(breaker), &val); - - return (val == magic); -} - +PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL); diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h new file mode 100644 index 000000000..768bf7f53 --- /dev/null +++ b/src/modules/systemlib/circuit_breaker_params.h @@ -0,0 +1,7 @@ +#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0 +#define PARAM_CBRK_RATE_CTRL_DEFAULT 0 +#define PARAM_CBRK_IO_SAFETY_DEFAULT 0 +#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0 +#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212 +#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953 +#define PARAM_CBRK_GPSFAIL_DEFAULT 240024 diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index fe8b7e75a..1e0a9c007 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -53,7 +53,8 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - circuit_breaker.c \ + circuit_breaker.cpp \ + circuit_breaker_params.c \ mcu_version.c MAXOPTIMIZATION = -Os diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index d06606a5d..49590c086 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -40,6 +40,7 @@ #define _SYSTEMLIB_PERF_COUNTER_H value #include <stdint.h> +#include <platforms/px4_defines.h> /** * Counter types. diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 71757e1f4..41a866968 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -49,15 +49,16 @@ #include "topics/actuator_direct.h" #include "topics/encoders.h" #include "topics/tecs_status.h" +#include "topics/rc_channels.h" namespace uORB { template<class T> Publication<T>::Publication( - List<PublicationBase *> * list, - const struct orb_metadata *meta) : + const struct orb_metadata *meta, + List<PublicationNode *> * list) : T(), // initialize data structure to zero - PublicationBase(list, meta) { + PublicationNode(meta, list) { } template<class T> @@ -80,5 +81,6 @@ template class __EXPORT Publication<actuator_outputs_s>; template class __EXPORT Publication<actuator_direct_s>; template class __EXPORT Publication<encoders_s>; template class __EXPORT Publication<tecs_status_s>; +template class __EXPORT Publication<rc_channels_s>; } diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8650b3df8..fd1ee4dec 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -38,6 +38,8 @@ #pragma once +#include <assert.h> + #include <uORB/uORB.h> #include <containers/List.hpp> @@ -49,55 +51,112 @@ namespace uORB * Base publication warapper class, used in list traversal * of various publications. */ -class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *> +class __EXPORT PublicationBase { public: - PublicationBase( - List<PublicationBase *> * list, - const struct orb_metadata *meta) : + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + */ + PublicationBase(const struct orb_metadata *meta) : _meta(meta), _handle(-1) { - if (list != NULL) list->add(this); } - void update() { + + /** + * Update the struct + * @param data The uORB message struct we are updating. + */ + void update(void * data) { if (_handle > 0) { - orb_publish(getMeta(), getHandle(), getDataVoidPtr()); + orb_publish(getMeta(), getHandle(), data); } else { - setHandle(orb_advertise(getMeta(), getDataVoidPtr())); + setHandle(orb_advertise(getMeta(), data)); } } - virtual void *getDataVoidPtr() = 0; + + /** + * Deconstructor + */ virtual ~PublicationBase() { orb_unsubscribe(getHandle()); } +// accessors const struct orb_metadata *getMeta() { return _meta; } int getHandle() { return _handle; } protected: +// accessors void setHandle(orb_advert_t handle) { _handle = handle; } +// attributes const struct orb_metadata *_meta; orb_advert_t _handle; }; /** + * alias class name so it is clear that the base class + * can be used by itself if desired + */ +typedef PublicationBase PublicationTiny; + +/** + * The publication base class as a list node. + */ +class __EXPORT PublicationNode : + public PublicationBase, + public ListNode<PublicationNode *> +{ +public: + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * @param list A pointer to a list of subscriptions + * that this should be appended to. + */ + PublicationNode(const struct orb_metadata *meta, + List<PublicationNode *> * list=nullptr) : + PublicationBase(meta) { + if (list != nullptr) list->add(this); + } + + /** + * This function is the callback for list traversal + * updates, a child class must implement it. + */ + virtual void update() = 0; +}; + +/** * Publication wrapper class */ template<class T> class Publication : public T, // this must be first! - public PublicationBase + public PublicationNode { public: /** * Constructor * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param list A list interface for adding to + * list during construction */ - Publication(List<PublicationBase *> * list, - const struct orb_metadata *meta); + Publication(const struct orb_metadata *meta, + List<PublicationNode *> * list=nullptr); + + /** + * Deconstructor + **/ virtual ~Publication(); + /* * XXX * This function gets the T struct, assuming @@ -106,6 +165,13 @@ public: * seem to be available */ void *getDataVoidPtr(); + + /** + * Create an update function that uses the embedded struct. + */ + void update() { + PublicationBase::update(getDataVoidPtr()); + } }; } // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 44b6debc7..fa0594c2e 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -52,25 +52,18 @@ #include "topics/vehicle_local_position.h" #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" +#include "topics/rc_channels.h" namespace uORB { -bool __EXPORT SubscriptionBase::updated() -{ - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; -} - template<class T> Subscription<T>::Subscription( - List<SubscriptionBase *> * list, - const struct orb_metadata *meta, unsigned interval) : + const struct orb_metadata *meta, + unsigned interval, + List<SubscriptionNode *> * list) : T(), // initialize data structure to zero - SubscriptionBase(list, meta) { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); + SubscriptionNode(meta, interval, list) { } template<class T> @@ -101,5 +94,8 @@ template class __EXPORT Subscription<vehicle_local_position_setpoint_s>; template class __EXPORT Subscription<vehicle_local_position_s>; template class __EXPORT Subscription<vehicle_attitude_setpoint_s>; template class __EXPORT Subscription<vehicle_rates_setpoint_s>; +template class __EXPORT Subscription<rc_channels_s>; +template class __EXPORT Subscription<vehicle_control_mode_s>; +template class __EXPORT Subscription<actuator_armed_s>; } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 34e9a83e0..f82586285 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -38,10 +38,11 @@ #pragma once +#include <assert.h> + #include <uORB/uORB.h> #include <containers/List.hpp> - namespace uORB { @@ -49,8 +50,7 @@ namespace uORB * Base subscription warapper class, used in list traversal * of various subscriptions. */ -class __EXPORT SubscriptionBase : - public ListNode<SubscriptionBase *> +class __EXPORT SubscriptionBase { public: // methods @@ -58,23 +58,42 @@ public: /** * Constructor * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * + * @param interval The minimum interval in milliseconds + * between updates */ - SubscriptionBase( - List<SubscriptionBase *> * list, - const struct orb_metadata *meta) : + SubscriptionBase(const struct orb_metadata *meta, + unsigned interval=0) : _meta(meta), _handle() { - if (list != NULL) list->add(this); + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); } - bool updated(); - void update() { + + /** + * Check if there is a new update. + * */ + bool updated() { + bool isUpdated = false; + orb_check(_handle, &isUpdated); + return isUpdated; + } + + /** + * Update the struct + * @param data The uORB message struct we are updating. + */ + void update(void * data) { if (updated()) { - orb_copy(_meta, _handle, getDataVoidPtr()); + orb_copy(_meta, _handle, data); } } - virtual void *getDataVoidPtr() = 0; + + /** + * Deconstructor + */ virtual ~SubscriptionBase() { orb_unsubscribe(_handle); } @@ -90,30 +109,86 @@ protected: }; /** + * alias class name so it is clear that the base class + */ +typedef SubscriptionBase SubscriptionTiny; + +/** + * The publication base class as a list node. + */ +class __EXPORT SubscriptionNode : + + public SubscriptionBase, + public ListNode<SubscriptionNode *> +{ +public: + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * @param interval The minimum interval in milliseconds + * between updates + * @param list A pointer to a list of subscriptions + * that this should be appended to. + */ + SubscriptionNode(const struct orb_metadata *meta, + unsigned interval=0, + List<SubscriptionNode *> * list=nullptr) : + SubscriptionBase(meta, interval), + _interval(interval) { + if (list != nullptr) list->add(this); + } + + /** + * This function is the callback for list traversal + * updates, a child class must implement it. + */ + virtual void update() = 0; +// accessors + unsigned getInterval() { return _interval; } +protected: +// attributes + unsigned _interval; + +}; + +/** * Subscription wrapper class */ template<class T> class __EXPORT Subscription : public T, // this must be first! - public SubscriptionBase + public SubscriptionNode { public: /** * Constructor * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @param interval The minimum interval in milliseconds between updates + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param interval The minimum interval in milliseconds + * between updates + * @param list A list interface for adding to + * list during construction */ - Subscription( - List<SubscriptionBase *> * list, - const struct orb_metadata *meta, unsigned interval); + Subscription(const struct orb_metadata *meta, + unsigned interval=0, + List<SubscriptionNode *> * list=nullptr); /** * Deconstructor */ virtual ~Subscription(); + + /** + * Create an update function that uses the embedded struct. + */ + void update() { + SubscriptionBase::update(getDataVoidPtr()); + } + /* * XXX * This function gets the T struct, assuming diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h deleted file mode 100644 index 4d1f9a638..000000000 --- a/src/modules/uORB/topics/actuator_controls.h +++ /dev/null @@ -1,81 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file actuator_controls.h - * - * Actuator control topics - mixer inputs. - * - * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. - * - * Each topic can be published by a single controller - */ - -#ifndef TOPIC_ACTUATOR_CONTROLS_H -#define TOPIC_ACTUATOR_CONTROLS_H - -#include <stdint.h> -#include "../uORB.h" - -#define NUM_ACTUATOR_CONTROLS 8 -#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ - -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) - -/** - * @addtogroup topics - * @{ - */ - -struct actuator_controls_s { - uint64_t timestamp; - float control[NUM_ACTUATOR_CONTROLS]; -}; - -/** - * @} - */ - -/* actuator control sets; this list can be expanded as more controllers emerge */ -ORB_DECLARE(actuator_controls_0); -ORB_DECLARE(actuator_controls_1); -ORB_DECLARE(actuator_controls_2); -ORB_DECLARE(actuator_controls_3); -ORB_DECLARE(actuator_controls_virtual_mc); -ORB_DECLARE(actuator_controls_virtual_fw); - - -#endif diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index d2ee754cd..676c37c77 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -40,7 +40,7 @@ #ifndef TOPIC_AIRSPEED_H_ #define TOPIC_AIRSPEED_H_ -#include "../uORB.h" +#include <platforms/px4_defines.h> #include <stdint.h> /** diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h index 6f16c51cf..a61f078ba 100644 --- a/src/modules/uORB/topics/fence.h +++ b/src/modules/uORB/topics/fence.h @@ -42,7 +42,7 @@ #include <stdint.h> #include <stdbool.h> -#include "../uORB.h" +#include <platforms/px4_defines.h> /** * @addtogroup topics diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h deleted file mode 100644 index 50b7bd9e5..000000000 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file manual_control_setpoint.h - * Definition of the manual_control_setpoint uORB topic. - */ - -#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_ -#define TOPIC_MANUAL_CONTROL_SETPOINT_H_ - -#include <stdint.h> -#include "../uORB.h" - -/** - * Switch position - */ -typedef enum { - SWITCH_POS_NONE = 0, /**< switch is not mapped */ - SWITCH_POS_ON, /**< switch activated (value = 1) */ - SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ - SWITCH_POS_OFF /**< switch not activated (value = -1) */ -} switch_pos_t; - -/** - * @addtogroup topics - * @{ - */ - -struct manual_control_setpoint_s { - uint64_t timestamp; - - /** - * Any of the channels may not be available and be set to NaN - * to indicate that it does not contain valid data. - * The variable names follow the definition of the - * MANUAL_CONTROL mavlink message. - * The default range is from -1 to 1 (mavlink message -1000 to 1000) - * The range for the z variable is defined from 0 to 1. (The z field of - * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) - */ - float x; /**< stick position in x direction -1..1 - in general corresponds to forward/back motion or pitch of vehicle, - in general a positive value means forward or negative pitch and - a negative value means backward or positive pitch */ - float y; /**< stick position in y direction -1..1 - in general corresponds to right/left motion or roll of vehicle, - in general a positive value means right or positive roll and - a negative value means left or negative roll */ - float z; /**< throttle stick position 0..1 - in general corresponds to up/down motion or thrust of vehicle, - in general the value corresponds to the demanded throttle by the user, - if the input is used for setting the setpoint of a vertical position - controller any value > 0.5 means up and any value < 0.5 means down */ - float r; /**< yaw stick/twist positon, -1..1 - in general corresponds to the righthand rotation around the vertical - (downwards) axis of the vehicle */ - float flaps; /**< flap position */ - float aux1; /**< default function: camera yaw / azimuth */ - float aux2; /**< default function: camera pitch / tilt */ - float aux3; /**< default function: camera trigger */ - float aux4; /**< default function: camera roll */ - float aux5; /**< default function: payload drop */ - - switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ - switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ - switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ - switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ - switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ - switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(manual_control_setpoint); - -#endif diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h deleted file mode 100644 index 16916cc4d..000000000 --- a/src/modules/uORB/topics/rc_channels.h +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file rc_channels.h - * Definition of the rc_channels uORB topic. - * - * @deprecated DO NOT USE FOR NEW CODE - */ - -#ifndef RC_CHANNELS_H_ -#define RC_CHANNELS_H_ - -#include <stdint.h> -#include "../uORB.h" - -/** - * This defines the mapping of the RC functions. - * The value assigned to the specific function corresponds to the entry of - * the channel array channels[]. - */ -enum RC_CHANNELS_FUNCTION { - THROTTLE = 0, - ROLL, - PITCH, - YAW, - MODE, - RETURN, - POSCTL, - LOITER, - OFFBOARD, - ACRO, - FLAPS, - AUX_1, - AUX_2, - AUX_3, - AUX_4, - AUX_5 -}; - -// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS - -#define RC_CHANNELS_FUNCTION_MAX 18 - -/** - * @addtogroup topics - * @{ - */ -struct rc_channels_s { - uint64_t timestamp; /**< Timestamp in microseconds since boot time */ - uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */ - float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */ - uint8_t channel_count; /**< Number of valid channels */ - int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */ - uint8_t rssi; /**< Receive signal strength index */ - bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */ -}; /**< radio control channels. */ - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(rc_channels); - -#endif diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h deleted file mode 100755 index 77324415a..000000000 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ /dev/null @@ -1,109 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_attitude.h - * Definition of the attitude uORB topic. - */ - -#ifndef VEHICLE_ATTITUDE_H_ -#define VEHICLE_ATTITUDE_H_ - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Attitude in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - */ -struct vehicle_attitude_s { - - uint64_t timestamp; /**< in microseconds since system start */ - - /* This is similar to the mavlink message ATTITUDE, but for onboard use */ - - /** @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */ - - float roll; /**< Roll angle (rad, Tait-Bryan, NED) */ - float pitch; /**< Pitch angle (rad, Tait-Bryan, NED) */ - float yaw; /**< Yaw angle (rad, Tait-Bryan, NED) */ - float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ - float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ - float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ - float rollacc; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ - float pitchacc; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ - float yawacc; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ - float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ - float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ - float q[4]; /**< Quaternion (NED) */ - float g_comp[3]; /**< Compensated gravity vector */ - bool R_valid; /**< Rotation matrix valid */ - bool q_valid; /**< Quaternion valid */ - - // secondary attitude, use for VTOL - float roll_sec; /**< Roll angle (rad, Tait-Bryan, NED) */ - float pitch_sec; /**< Pitch angle (rad, Tait-Bryan, NED) */ - float yaw_sec; /**< Yaw angle (rad, Tait-Bryan, NED) */ - float rollspeed_sec; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ - float pitchspeed_sec; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ - float yawspeed_sec; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ - float rollacc_sec; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ - float pitchacc_sec; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ - float yawacc_sec; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ - float rate_offsets_sec[3]; /**< Offsets of the body angular rates from zero */ - float R_sec[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ - float q_sec[4]; /**< Quaternion (NED) */ - float g_comp_sec[3]; /**< Compensated gravity vector */ - bool R_valid_sec; /**< Rotation matrix valid */ - bool q_valid_sec; /**< Quaternion valid */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_attitude); - -#endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h deleted file mode 100644 index 1cfc37cc6..000000000 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ /dev/null @@ -1,89 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_attitude_setpoint.h - * Definition of the vehicle attitude setpoint uORB topic. - */ - -#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ -#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * vehicle attitude setpoint. - */ -struct vehicle_attitude_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - float roll_body; /**< body angle in NED frame */ - float pitch_body; /**< body angle in NED frame */ - float yaw_body; /**< body angle in NED frame */ - //float body_valid; /**< Set to true if body angles are valid */ - - float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ - bool R_valid; /**< Set to true if rotation matrix is valid */ - - //! For quaternion-based attitude control - float q_d[4]; /** Desired quaternion for quaternion control */ - bool q_d_valid; /**< Set to true if quaternion vector is valid */ - float q_e[4]; /** Attitude error in quaternion */ - bool q_e_valid; /**< Set to true if quaternion error vector is valid */ - - float thrust; /**< Thrust in Newton the power system should generate */ - - bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ - bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */ - bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_attitude_setpoint); -ORB_DECLARE(mc_virtual_attitude_setpoint); -ORB_DECLARE(fw_virtual_attitude_setpoint); - -#endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h deleted file mode 100644 index ca7705456..000000000 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ /dev/null @@ -1,95 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_control_mode.h - * Definition of the vehicle_control_mode uORB topic. - * - * All control apps should depend their actions based on the flags set here. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - */ - -#ifndef VEHICLE_CONTROL_MODE -#define VEHICLE_CONTROL_MODE - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" -#include "vehicle_status.h" - -/** - * @addtogroup topics @{ - */ - - -/** - * state machine / state of vehicle. - * - * Encodes the complete system state and is set by the commander app. - */ - -struct vehicle_control_mode_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - bool flag_armed; - - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - - // XXX needs yet to be set by state machine helper - bool flag_system_hil_enabled; - - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ - bool flag_control_offboard_enabled; /**< true if offboard control should be used */ - bool flag_control_rates_enabled; /**< true if rates are stabilized */ - bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_force_enabled; /**< true if force control is mixed in */ - bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_control_altitude_enabled; /**< true if altitude is controlled */ - bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_control_mode); - -#endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index c3bb3b893..f7a432495 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -45,7 +45,7 @@ #include <stdint.h> #include <stdbool.h> -#include "../uORB.h" +#include <platforms/px4_defines.h> /** * @addtogroup topics diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h deleted file mode 100644 index 47d51f199..000000000 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_rates_setpoint.h - * Definition of the vehicle rates setpoint topic - */ - -#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_ -#define TOPIC_VEHICLE_RATES_SETPOINT_H_ - -#include <stdint.h> -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ -struct vehicle_rates_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start */ - - float roll; /**< body angular rates in NED frame */ - float pitch; /**< body angular rates in NED frame */ - float yaw; /**< body angular rates in NED frame */ - float thrust; /**< thrust normalized to 0..1 */ - -}; /**< vehicle_rates_setpoint */ - -/** -* @} -*/ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_rates_setpoint); -ORB_DECLARE(mc_virtual_rates_setpoint); -ORB_DECLARE(fw_virtual_rates_setpoint); -#endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h deleted file mode 100644 index 749d00d75..000000000 --- a/src/modules/uORB/topics/vehicle_status.h +++ /dev/null @@ -1,259 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_status.h - * Definition of the vehicle_status uORB topic. - * - * Published the state machine and the system status bitfields - * (see SYS_STATUS mavlink message), published only by commander app. - * - * All apps should write to subsystem_info: - * - * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) - * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <julian@oes.ch> - */ - -#ifndef VEHICLE_STATUS_H_ -#define VEHICLE_STATUS_H_ - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" - -/** - * @addtogroup topics @{ - */ - -/** - * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. - */ -typedef enum { - MAIN_STATE_MANUAL = 0, - MAIN_STATE_ALTCTL, - MAIN_STATE_POSCTL, - MAIN_STATE_AUTO_MISSION, - MAIN_STATE_AUTO_LOITER, - MAIN_STATE_AUTO_RTL, - MAIN_STATE_ACRO, - MAIN_STATE_OFFBOARD, - MAIN_STATE_MAX -} main_state_t; - -// If you change the order, add or remove arming_state_t states make sure to update the arrays -// in state_machine_helper.cpp as well. -typedef enum { - ARMING_STATE_INIT = 0, - ARMING_STATE_STANDBY, - ARMING_STATE_ARMED, - ARMING_STATE_ARMED_ERROR, - ARMING_STATE_STANDBY_ERROR, - ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE, - ARMING_STATE_MAX, -} arming_state_t; - -typedef enum { - HIL_STATE_OFF = 0, - HIL_STATE_ON -} hil_state_t; - -/** - * Navigation state, i.e. "what should vehicle do". - */ -typedef enum { - NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ - NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ - NAVIGATION_STATE_POSCTL, /**< Position control mode */ - NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ - NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ - NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ - NAVIGATION_STATE_AUTO_RCRECOVER, /**< RC recover mode */ - NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ - NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ - NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ - NAVIGATION_STATE_ACRO, /**< Acro mode */ - NAVIGATION_STATE_LAND, /**< Land mode */ - NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ - NAVIGATION_STATE_TERMINATION, /**< Termination mode */ - NAVIGATION_STATE_OFFBOARD, - NAVIGATION_STATE_MAX, -} navigation_state_t; - -enum VEHICLE_MODE_FLAG { - VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, - VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, - VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, - VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, - VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, - VEHICLE_MODE_FLAG_TEST_ENABLED = 2, - VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 -}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ - -/** - * Should match 1:1 MAVLink's MAV_TYPE ENUM - */ -enum VEHICLE_TYPE { - VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */ - VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */ - VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */ - VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */ - VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */ - VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */ - VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */ - VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */ - VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */ - VEHICLE_TYPE_ROCKET = 9, /* Rocket | */ - VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */ - VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */ - VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */ - VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */ - VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */ - VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */ - VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */ - VEHICLE_TYPE_KITE = 17, /* Kite | */ - VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ - VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */ - VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/ - VEHICLE_TYPE_ENUM_END = 21 /* | */ -}; - -enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ - VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ -}; - -/** - * @addtogroup topics - * @{ - */ - -/** - * state machine / state of vehicle. - * - * Encodes the complete system state and is set by the commander app. - */ -struct vehicle_status_s { - /* use of a counter and timestamp recommended (but not necessary) */ - - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - main_state_t main_state; /**< main state machine */ - navigation_state_t nav_state; /**< set navigation state machine to specified value */ - arming_state_t arming_state; /**< current arming state */ - hil_state_t hil_state; /**< current hil state */ - bool failsafe; /**< true if system is in failsafe state */ - - int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ - int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ - int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ - - bool is_rotary_wing; - - bool condition_battery_voltage_valid; - bool condition_system_in_air_restore; /**< true if we can restore in mid air */ - bool condition_system_sensors_initialized; - bool condition_system_returned_to_home; - bool condition_auto_mission_available; - bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */ - bool condition_launch_position_valid; /**< indicates a valid launch position */ - bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ - bool condition_local_position_valid; - bool condition_local_altitude_valid; - bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ - bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ - bool condition_power_input_valid; /**< set if input power is valid */ - float avionics_power_rail_voltage; /**< voltage of the avionics power rail */ - - bool rc_signal_found_once; - bool rc_signal_lost; /**< true if RC reception lost */ - uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */ - bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */ - bool rc_input_blocked; /**< set if RC input should be ignored */ - - bool data_link_lost; /**< datalink to GCS lost */ - bool data_link_lost_cmd; /**< datalink to GCS lost mode commanded */ - uint8_t data_link_lost_counter; /**< counts unique data link lost events */ - bool engine_failure; /** Set to true if an engine failure is detected */ - bool engine_failure_cmd; /** Set to true if an engine failure mode is commanded */ - bool gps_failure; /** Set to true if a gps failure is detected */ - bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */ - - bool barometer_failure; /** Set to true if a barometer failure is detected */ - - bool offboard_control_signal_found_once; - bool offboard_control_signal_lost; - bool offboard_control_signal_weak; - uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ - bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command - and should not be overridden by RC */ - - /* see SYS_STATUS mavlink message for the following */ - uint32_t onboard_control_sensors_present; - uint32_t onboard_control_sensors_enabled; - uint32_t onboard_control_sensors_health; - - float load; /**< processor load from 0 to 1 */ - float battery_voltage; - float battery_current; - float battery_remaining; - - enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */ - uint16_t drop_rate_comm; - uint16_t errors_comm; - uint16_t errors_count1; - uint16_t errors_count2; - uint16_t errors_count3; - uint16_t errors_count4; - - bool circuit_breaker_engaged_power_check; - bool circuit_breaker_engaged_airspd_check; - bool circuit_breaker_engaged_enginefailure_check; - bool circuit_breaker_engaged_gpsfailure_check; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_status); - -#endif diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 82ff46ad2..a6e9171ed 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -125,7 +125,7 @@ typedef intptr_t orb_advert_t; * node in /obj if required and publishes the initial data. * * Any number of advertisers may publish to a topic; publications are atomic - * but co-ordination between publishers is not provided by the ORB. + * but co-ordination between publishers is not provided by the ORB. * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. @@ -261,4 +261,31 @@ extern int orb_set_interval(int handle, unsigned interval) __EXPORT; __END_DECLS +/* Diverse uORB header defines */ //XXX: move to better location +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) +ORB_DECLARE(actuator_controls_0); +typedef struct actuator_controls_s actuator_controls_0_s; +ORB_DECLARE(actuator_controls_1); +typedef struct actuator_controls_s actuator_controls_1_s; +ORB_DECLARE(actuator_controls_2); +typedef struct actuator_controls_s actuator_controls_2_s; +ORB_DECLARE(actuator_controls_3); +typedef struct actuator_controls_s actuator_controls_3_s; +ORB_DECLARE(actuator_controls_virtual_mc); +typedef struct actuator_controls_s actuator_controls_virtual_mc_s; +ORB_DECLARE(actuator_controls_virtual_fw); +typedef struct actuator_controls_s actuator_controls_virtual_fw_s; +ORB_DECLARE(mc_virtual_rates_setpoint); +typedef struct vehicle_rates_setpoint_s mc_virtual_rates_setpoint_s; +ORB_DECLARE(fw_virtual_rates_setpoint); +typedef struct vehicle_rates_setpoint_s fw_virtual_rates_setpoint_s; +ORB_DECLARE(mc_virtual_attitude_setpoint); +typedef struct vehicle_attitude_setpoint_s mc_virtual_attitude_setpoint_s; +ORB_DECLARE(fw_virtual_attitude_setpoint); +typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s; +typedef uint8_t arming_state_t; +typedef uint8_t main_state_t; +typedef uint8_t hil_state_t; +typedef uint8_t navigation_state_t; + #endif /* _UORB_UORB_H */ |