From f347e87391d35d43bdf4bdf51323ea0c53e9ead7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 15:54:34 +0200 Subject: Added base class for fixed wing attitude controller -> still working on it --- src/modules/fw_att_control/fw_att_control_base.cpp | 50 +++++++++ src/modules/fw_att_control/fw_att_control_base.h | 124 +++++++++++++++++++++ 2 files changed, 174 insertions(+) create mode 100644 src/modules/fw_att_control/fw_att_control_base.cpp create mode 100644 src/modules/fw_att_control/fw_att_control_base.h (limited to 'src/modules/fw_att_control') 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..4f61f02ef --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -0,0 +1,50 @@ +/* + * fw_att_control_base.cpp + * + * Created on: Sep 24, 2014 + * Author: roman + */ + +#include "fw_att_control_base.h" + + +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 = {}; + _accel = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; + _vehicle_status = {}; + +} + + + +FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() +{ + +} + +void FixedwingAttitudeControlBase::control_attitude() +{ + +} 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..9a2d9b195 --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -0,0 +1,124 @@ +/* + * fw_att_control_base.h + * + * Created on: Sep 24, 2014 + * Author: roman + */ + +#ifndef FW_ATT_CONTROL_BASE_H_ +#define FW_ATT_CONTROL_BASE_H_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +class FixedwingAttitudeControlBase +{ +public: + /** + * Constructor + */ + FixedwingAttitudeControlBase(); + + /** + * Destructor, also kills the sensors task. + */ + ~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 accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct 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; + + + + +}; + + + +#endif /* FW_ATT_CONTROL_BASE_H_ */ -- cgit v1.2.3 From 43d9ebc231501fe2d3fe6541f1079d2d019a2698 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:28:49 +0200 Subject: Added control_attitude function and cleaned up --- src/modules/fw_att_control/fw_att_control_base.cpp | 223 +++++++++++++++++++-- 1 file changed, 206 insertions(+), 17 deletions(-) (limited to 'src/modules/fw_att_control') diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 4f61f02ef..7218e4e9b 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -6,23 +6,23 @@ */ #include "fw_att_control_base.h" +#include +#include +#include +#include +using namespace std; FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : - _task_should_exit(false), - _task_running(false), - _control_task(-1), + _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) -{ + /* 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 = {}; _accel = {}; @@ -37,14 +37,203 @@ FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : } +FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() { +} -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); + } + } -void FixedwingAttitudeControlBase::control_attitude() -{ + /* 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); + } + } } -- cgit v1.2.3 From 43056258839c67590dec811e923553fad66a0f4b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:29:30 +0200 Subject: Added control_attitude function --- src/modules/fw_att_control/fw_att_control_base.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules/fw_att_control') diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 9a2d9b195..6e071fe20 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -114,6 +114,8 @@ protected: ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; + void control_attitude(); + -- cgit v1.2.3 From c8b1c5b119e6552e8e5420ca8fecd24b2a3ad4a2 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Sat, 27 Sep 2014 12:01:11 +0200 Subject: Adapted for shared library use with ROS --- src/lib/geo/geo.h | 6 +- src/lib/geo_lookup/geo_mag_declination.h | 4 + src/modules/fw_att_control/fw_att_control_main.cpp | 419 +++++++++++---------- src/modules/uORB/topics/actuator_armed.h | 6 +- 4 files changed, 227 insertions(+), 208 deletions(-) (limited to 'src/modules/fw_att_control') diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 2311e0a7c..ff2d92389 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -45,14 +45,16 @@ #pragma once +#ifdef CONFIG_ARCH_ARM #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" __BEGIN_DECLS - +#endif #include "geo_lookup/geo_mag_declination.h" #include +#include #define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ #define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ @@ -276,4 +278,6 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); +#ifdef CONFIG_ARCH_ARM __END_DECLS +#endif diff --git a/src/lib/geo_lookup/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h index 0ac062d6d..d79b78412 100644 --- a/src/lib/geo_lookup/geo_mag_declination.h +++ b/src/lib/geo_lookup/geo_mag_declination.h @@ -40,8 +40,12 @@ #pragma once +#ifdef CONFIG_ARCH_ARM __BEGIN_DECLS __EXPORT float get_mag_declination(float lat, float lon); __END_DECLS +#else +float get_mag_declination(float lat, float lon); +#endif 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 c60d8d348..5cff390e2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -83,8 +83,7 @@ */ extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); -class FixedwingAttitudeControl -{ +class FixedwingAttitudeControl { public: /** * Constructor @@ -101,54 +100,56 @@ public: * * @return OK on success. */ - int start(); + int start(); /** * Task status * * @return true if the mainloop is running */ - bool task_running() { return _task_running; } + bool task_running() { + return _task_running; + } private: - 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 */ - - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ - int _global_pos_sub; /**< global position subscription */ - int _vehicle_status_sub; /**< vehicle status subscription */ - - orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ - orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ - orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ - - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct 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 */ + 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 */ + + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _global_pos_sub; /**< global position subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ + + orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct 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; @@ -182,14 +183,14 @@ private: 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 */ + 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 */ + } _parameters; /**< local copies of interesting parameters */ struct { @@ -228,75 +229,71 @@ private: param_t pitchsp_offset_deg; param_t man_roll_max; param_t man_pitch_max; - } _parameter_handles; /**< handles for interesting parameters */ - - - ECL_RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; + } _parameter_handles; /**< handles for interesting parameters */ + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; /** * Update our local parameter cache. */ - int parameters_update(); + int parameters_update(); /** * Update control outputs * */ - void control_update(); + void control_update(); /** * Check for changes in vehicle control mode. */ - void vehicle_control_mode_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in manual inputs. */ - void vehicle_manual_poll(); - + void vehicle_manual_poll(); /** * Check for airspeed updates. */ - void vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_accel_poll(); /** * Check for set triplet updates. */ - void vehicle_setpoint_poll(); + void vehicle_setpoint_poll(); /** * Check for global position updates. */ - void global_pos_poll(); + void global_pos_poll(); /** * Check for vehicle status updates. */ - void vehicle_status_poll(); + void vehicle_status_poll(); /** * Shim for calling task_main from task_create. */ - static void task_main_trampoline(int argc, char *argv[]); + static void task_main_trampoline(int argc, char *argv[]); /** * Main sensor collection task. */ - void task_main(); + void task_main(); }; -namespace att_control -{ +namespace att_control { /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -304,39 +301,28 @@ namespace att_control #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control = nullptr; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : - _task_should_exit(false), - _task_running(false), - _control_task(-1), - -/* subscriptions */ - _att_sub(-1), - _accel_sub(-1), - _airspeed_sub(-1), - _vcontrol_mode_sub(-1), - _params_sub(-1), - _manual_sub(-1), - _global_pos_sub(-1), - _vehicle_status_sub(-1), - -/* publications */ - _rate_sp_pub(-1), - _attitude_sp_pub(-1), - _actuators_0_pub(-1), - _actuators_1_pub(-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) -{ + _task_should_exit(false), _task_running(false), _control_task(-1), + + /* subscriptions */ + _att_sub(-1), _accel_sub(-1), _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub( + -1), _manual_sub(-1), _global_pos_sub(-1), _vehicle_status_sub( + -1), + + /* publications */ + _rate_sp_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), _actuators_1_pub( + -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 = {}; _accel = {}; @@ -349,7 +335,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _global_pos = {}; _vehicle_status = {}; - _parameter_handles.tconst = param_find("FW_ATT_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); @@ -390,8 +375,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : parameters_update(); } -FixedwingAttitudeControl::~FixedwingAttitudeControl() -{ +FixedwingAttitudeControl::~FixedwingAttitudeControl() { if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ @@ -419,9 +403,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() att_control::g_control = nullptr; } -int -FixedwingAttitudeControl::parameters_update() -{ +int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.tconst, &(_parameters.tconst)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); @@ -429,21 +411,26 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); - param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); - param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); + param_get(_parameter_handles.p_integrator_max, + &(_parameters.p_integrator_max)); + param_get(_parameter_handles.p_roll_feedforward, + &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); - param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); + param_get(_parameter_handles.r_integrator_max, + &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); - param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); - param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); + param_get(_parameter_handles.y_integrator_max, + &(_parameters.y_integrator_max)); + param_get(_parameter_handles.y_coordinated_min_speed, + &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); @@ -453,16 +440,19 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); - param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg)); - param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); - _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); - _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); + param_get(_parameter_handles.rollsp_offset_deg, + &(_parameters.rollsp_offset_deg)); + param_get(_parameter_handles.pitchsp_offset_deg, + &(_parameters.pitchsp_offset_deg)); + _parameters.rollsp_offset_rad = math::radians( + _parameters.rollsp_offset_deg); + _parameters.pitchsp_offset_rad = math::radians( + _parameters.pitchsp_offset_deg); param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); - /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -492,9 +482,7 @@ FixedwingAttitudeControl::parameters_update() return OK; } -void -FixedwingAttitudeControl::vehicle_control_mode_poll() -{ +void FixedwingAttitudeControl::vehicle_control_mode_poll() { bool vcontrol_mode_updated; /* Check HIL state if vehicle status has changed */ @@ -502,13 +490,12 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, + &_vcontrol_mode); } } -void -FixedwingAttitudeControl::vehicle_manual_poll() -{ +void FixedwingAttitudeControl::vehicle_manual_poll() { bool manual_updated; /* get pilots inputs */ @@ -520,9 +507,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -void -FixedwingAttitudeControl::vehicle_airspeed_poll() -{ +void FixedwingAttitudeControl::vehicle_airspeed_poll() { /* check if there is a new position */ bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -533,9 +518,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() } } -void -FixedwingAttitudeControl::vehicle_accel_poll() -{ +void FixedwingAttitudeControl::vehicle_accel_poll() { /* check if there is a new position */ bool accel_updated; orb_check(_accel_sub, &accel_updated); @@ -545,9 +528,7 @@ FixedwingAttitudeControl::vehicle_accel_poll() } } -void -FixedwingAttitudeControl::vehicle_setpoint_poll() -{ +void FixedwingAttitudeControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ bool att_sp_updated; orb_check(_att_sp_sub, &att_sp_updated); @@ -558,21 +539,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll() } } -void -FixedwingAttitudeControl::global_pos_poll() -{ +void FixedwingAttitudeControl::global_pos_poll() { /* check if there is a new global position */ bool global_pos_updated; orb_check(_global_pos_sub, &global_pos_updated); if (global_pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, + &_global_pos); } } -void -FixedwingAttitudeControl::vehicle_status_poll() -{ +void FixedwingAttitudeControl::vehicle_status_poll() { /* check if there is new status information */ bool vehicle_status_updated; orb_check(_vehicle_status_sub, &vehicle_status_updated); @@ -582,15 +560,11 @@ FixedwingAttitudeControl::vehicle_status_poll() } } -void -FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) -{ +void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { att_control::g_control->task_main(); } -void -FixedwingAttitudeControl::task_main() -{ +void FixedwingAttitudeControl::task_main() { /* inform about start */ warnx("Initializing.."); @@ -667,7 +641,6 @@ FixedwingAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -721,8 +694,8 @@ FixedwingAttitudeControl::task_main() 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) { + 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); @@ -740,16 +713,20 @@ FixedwingAttitudeControl::task_main() * 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 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) { + 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; + pitch_sp = _att_sp.pitch_body + + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -775,10 +752,12 @@ FixedwingAttitudeControl::task_main() * 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; + 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; @@ -797,11 +776,13 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); + orb_publish(ORB_ID(vehicle_attitude_setpoint), + _attitude_sp_pub, &att_sp); } else { /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + _attitude_sp_pub = orb_advertise( + ORB_ID(vehicle_attitude_setpoint), &att_sp); } } @@ -816,11 +797,17 @@ FixedwingAttitudeControl::task_main() 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 (_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"); } @@ -829,74 +816,94 @@ FixedwingAttitudeControl::task_main() /* 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); + _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 + _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; + _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); + warnx("roll_u %.4f", (double) roll_u); } } - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, + 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; + _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); + 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, + 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; + _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); + warnx("yaw_u %.4f", (double) yaw_u); } } /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + _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); + 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); + warnx( + "Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", + (double) roll_sp, (double) pitch_sp); } } @@ -913,11 +920,13 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, + &rates_sp); } else { /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), + &rates_sp); } } else { @@ -939,16 +948,19 @@ FixedwingAttitudeControl::task_main() if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, + &_actuators); } else { /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), + &_actuators); } if (_actuators_1_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, + &_actuators_airframe); // warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", // (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], // (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], @@ -956,7 +968,8 @@ FixedwingAttitudeControl::task_main() } else { /* advertise and publish */ - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), + &_actuators_airframe); } } @@ -972,18 +985,14 @@ FixedwingAttitudeControl::task_main() _exit(0); } -int -FixedwingAttitudeControl::start() -{ +int FixedwingAttitudeControl::start() { ASSERT(_control_task == -1); /* start the task */ _control_task = task_spawn_cmd("fw_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&FixedwingAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, 2048, + (main_t) &FixedwingAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { warn("task start failed"); @@ -993,8 +1002,7 @@ FixedwingAttitudeControl::start() return OK; } -int fw_att_control_main(int argc, char *argv[]) -{ +int fw_att_control_main(int argc, char *argv[]) { if (argc < 1) errx(1, "usage: fw_att_control {start|stop|status}"); @@ -1015,7 +1023,8 @@ int fw_att_control_main(int argc, char *argv[]) } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { + while (att_control::g_control == nullptr + || !att_control::g_control->task_running()) { usleep(50000); printf("."); fflush(stdout); diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index 0f6c9aca1..9ec9d10ab 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -42,8 +42,9 @@ #define TOPIC_ACTUATOR_ARMED_H #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" - +#endif /** * @addtogroup topics * @{ @@ -64,6 +65,7 @@ struct actuator_armed_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(actuator_armed); - +#endif #endif -- cgit v1.2.3 From 43ebaf287c86acbf7fba13b5203de68afcc79b44 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:06:14 +0100 Subject: cleaned up --- src/modules/fw_att_control/fw_att_control_base.cpp | 39 ++++++++++++-- src/modules/fw_att_control/fw_att_control_base.h | 59 +++++++++++++++------- 2 files changed, 75 insertions(+), 23 deletions(-) (limited to 'src/modules/fw_att_control') diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 7218e4e9b..d8ba15969 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -1,8 +1,39 @@ -/* - * fw_att_control_base.cpp +/* 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 * - * Created on: Sep 24, 2014 - * Author: roman */ #include "fw_att_control_base.h" diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 6e071fe20..6b2efc46b 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -1,13 +1,44 @@ -/* - * fw_att_control_base.h - * - * Created on: Sep 24, 2014 - * Author: roman - */ - #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 + * + */ + #include #include #include @@ -22,8 +53,8 @@ #include #include #include -#include +#include #include class FixedwingAttitudeControlBase @@ -35,7 +66,7 @@ public: FixedwingAttitudeControlBase(); /** - * Destructor, also kills the sensors task. + * Destructor */ ~FixedwingAttitudeControlBase(); @@ -46,8 +77,6 @@ protected: 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 accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -107,20 +136,12 @@ protected: } _parameters; /**< local copies of interesting parameters */ - - - ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; void control_attitude(); - - - }; - - #endif /* FW_ATT_CONTROL_BASE_H_ */ -- cgit v1.2.3 From 7e350555ea634013f083bccc2f5a8108fc5ef9f9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 12:36:05 +0100 Subject: checked out from PX4 master to avoid eclipse formatting which happened in the past --- src/modules/fw_att_control/fw_att_control_main.cpp | 414 ++++++++++----------- 1 file changed, 203 insertions(+), 211 deletions(-) (limited to 'src/modules/fw_att_control') 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 80b58ec71..e770c11a2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -83,7 +83,8 @@ */ extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); -class FixedwingAttitudeControl { +class FixedwingAttitudeControl +{ public: /** * Constructor @@ -100,56 +101,54 @@ public: * * @return OK on success. */ - int start(); + int start(); /** * Task status * * @return true if the mainloop is running */ - bool task_running() { - return _task_running; - } + bool task_running() { return _task_running; } private: - 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 */ - - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ - int _global_pos_sub; /**< global position subscription */ - int _vehicle_status_sub; /**< vehicle status subscription */ - - orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ - orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ - orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ - - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct 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 */ + 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 */ + + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _global_pos_sub; /**< global position subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ + + orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct 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; @@ -183,14 +182,14 @@ private: 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 */ + 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 */ + } _parameters; /**< local copies of interesting parameters */ struct { @@ -229,71 +228,75 @@ private: param_t pitchsp_offset_deg; param_t man_roll_max; param_t man_pitch_max; - } _parameter_handles; /**< handles for interesting parameters */ + } _parameter_handles; /**< handles for interesting parameters */ + + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; - ECL_RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; /** * Update our local parameter cache. */ - int parameters_update(); + int parameters_update(); /** * Update control outputs * */ - void control_update(); + void control_update(); /** * Check for changes in vehicle control mode. */ - void vehicle_control_mode_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in manual inputs. */ - void vehicle_manual_poll(); + void vehicle_manual_poll(); + /** * Check for airspeed updates. */ - void vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_accel_poll(); /** * Check for set triplet updates. */ - void vehicle_setpoint_poll(); + void vehicle_setpoint_poll(); /** * Check for global position updates. */ - void global_pos_poll(); + void global_pos_poll(); /** * Check for vehicle status updates. */ - void vehicle_status_poll(); + void vehicle_status_poll(); /** * Shim for calling task_main from task_create. */ - static void task_main_trampoline(int argc, char *argv[]); + static void task_main_trampoline(int argc, char *argv[]); /** * Main sensor collection task. */ - void task_main(); + void task_main(); }; -namespace att_control { +namespace att_control +{ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -301,28 +304,39 @@ namespace att_control { #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control = nullptr; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : - _task_should_exit(false), _task_running(false), _control_task(-1), - - /* subscriptions */ - _att_sub(-1), _accel_sub(-1), _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub( - -1), _manual_sub(-1), _global_pos_sub(-1), _vehicle_status_sub( - -1), - - /* publications */ - _rate_sp_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), _actuators_1_pub( - -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) { + _task_should_exit(false), + _task_running(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _accel_sub(-1), + _airspeed_sub(-1), + _vcontrol_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _global_pos_sub(-1), + _vehicle_status_sub(-1), + +/* publications */ + _rate_sp_pub(-1), + _attitude_sp_pub(-1), + _actuators_0_pub(-1), + _actuators_1_pub(-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 = {}; _accel = {}; @@ -335,6 +349,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _global_pos = {}; _vehicle_status = {}; + _parameter_handles.tconst = param_find("FW_ATT_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); @@ -375,7 +390,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : parameters_update(); } -FixedwingAttitudeControl::~FixedwingAttitudeControl() { +FixedwingAttitudeControl::~FixedwingAttitudeControl() +{ if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ @@ -403,7 +419,9 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() { att_control::g_control = nullptr; } -int FixedwingAttitudeControl::parameters_update() { +int +FixedwingAttitudeControl::parameters_update() +{ param_get(_parameter_handles.tconst, &(_parameters.tconst)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); @@ -411,26 +429,21 @@ int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); - param_get(_parameter_handles.p_integrator_max, - &(_parameters.p_integrator_max)); - param_get(_parameter_handles.p_roll_feedforward, - &(_parameters.p_roll_feedforward)); + param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); + param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); - param_get(_parameter_handles.r_integrator_max, - &(_parameters.r_integrator_max)); + param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); - param_get(_parameter_handles.y_integrator_max, - &(_parameters.y_integrator_max)); - param_get(_parameter_handles.y_coordinated_min_speed, - &(_parameters.y_coordinated_min_speed)); + param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); + param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); @@ -440,19 +453,16 @@ int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); - param_get(_parameter_handles.rollsp_offset_deg, - &(_parameters.rollsp_offset_deg)); - param_get(_parameter_handles.pitchsp_offset_deg, - &(_parameters.pitchsp_offset_deg)); - _parameters.rollsp_offset_rad = math::radians( - _parameters.rollsp_offset_deg); - _parameters.pitchsp_offset_rad = math::radians( - _parameters.pitchsp_offset_deg); + param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg)); + param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); + _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); + _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -482,7 +492,9 @@ int FixedwingAttitudeControl::parameters_update() { return OK; } -void FixedwingAttitudeControl::vehicle_control_mode_poll() { +void +FixedwingAttitudeControl::vehicle_control_mode_poll() +{ bool vcontrol_mode_updated; /* Check HIL state if vehicle status has changed */ @@ -490,12 +502,13 @@ void FixedwingAttitudeControl::vehicle_control_mode_poll() { if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, - &_vcontrol_mode); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } -void FixedwingAttitudeControl::vehicle_manual_poll() { +void +FixedwingAttitudeControl::vehicle_manual_poll() +{ bool manual_updated; /* get pilots inputs */ @@ -507,7 +520,9 @@ void FixedwingAttitudeControl::vehicle_manual_poll() { } } -void FixedwingAttitudeControl::vehicle_airspeed_poll() { +void +FixedwingAttitudeControl::vehicle_airspeed_poll() +{ /* check if there is a new position */ bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -518,7 +533,9 @@ void FixedwingAttitudeControl::vehicle_airspeed_poll() { } } -void FixedwingAttitudeControl::vehicle_accel_poll() { +void +FixedwingAttitudeControl::vehicle_accel_poll() +{ /* check if there is a new position */ bool accel_updated; orb_check(_accel_sub, &accel_updated); @@ -528,7 +545,9 @@ void FixedwingAttitudeControl::vehicle_accel_poll() { } } -void FixedwingAttitudeControl::vehicle_setpoint_poll() { +void +FixedwingAttitudeControl::vehicle_setpoint_poll() +{ /* check if there is a new setpoint */ bool att_sp_updated; orb_check(_att_sp_sub, &att_sp_updated); @@ -539,18 +558,21 @@ void FixedwingAttitudeControl::vehicle_setpoint_poll() { } } -void FixedwingAttitudeControl::global_pos_poll() { +void +FixedwingAttitudeControl::global_pos_poll() +{ /* check if there is a new global position */ bool global_pos_updated; orb_check(_global_pos_sub, &global_pos_updated); if (global_pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, - &_global_pos); + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } } -void FixedwingAttitudeControl::vehicle_status_poll() { +void +FixedwingAttitudeControl::vehicle_status_poll() +{ /* check if there is new status information */ bool vehicle_status_updated; orb_check(_vehicle_status_sub, &vehicle_status_updated); @@ -560,11 +582,15 @@ void FixedwingAttitudeControl::vehicle_status_poll() { } } -void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { +void +FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ att_control::g_control->task_main(); } -void FixedwingAttitudeControl::task_main() { +void +FixedwingAttitudeControl::task_main() +{ /* inform about start */ warnx("Initializing.."); @@ -641,6 +667,7 @@ void FixedwingAttitudeControl::task_main() { /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { + static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -694,8 +721,8 @@ void FixedwingAttitudeControl::task_main() { 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) { + 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); @@ -713,9 +740,7 @@ void FixedwingAttitudeControl::task_main() { * 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 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; @@ -731,8 +756,7 @@ void FixedwingAttitudeControl::task_main() { !_vcontrol_mode.flag_control_manual_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; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -758,12 +782,10 @@ void FixedwingAttitudeControl::task_main() { * 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; + 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; @@ -782,13 +804,11 @@ void FixedwingAttitudeControl::task_main() { /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), - _attitude_sp_pub, &att_sp); + orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); } else { /* advertise and publish */ - _attitude_sp_pub = orb_advertise( - ORB_ID(vehicle_attitude_setpoint), &att_sp); + _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } } @@ -803,17 +823,11 @@ void FixedwingAttitudeControl::task_main() { 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(_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"); } @@ -822,81 +836,63 @@ void FixedwingAttitudeControl::task_main() { /* 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); + _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 + _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; + _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); + warnx("roll_u %.4f", (double)roll_u); } } - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, - _att.pitch, _att.pitchspeed, _att.yawspeed, + 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; + _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); + 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, + 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; + _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); + warnx("yaw_u %.4f", (double)yaw_u); } } - /* throttle passed through if it is finite and if no engine failure was * detected */ _actuators.control[3] = (isfinite(throttle_sp) && @@ -905,15 +901,13 @@ void FixedwingAttitudeControl::task_main() { throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { - warnx("throttle_sp %.4f", (double) throttle_sp); + 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); + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } @@ -930,13 +924,11 @@ void FixedwingAttitudeControl::task_main() { if (_rate_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, - &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); } else { /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), - &rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); } } else { @@ -958,19 +950,16 @@ void FixedwingAttitudeControl::task_main() { if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, - &_actuators); + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), - &_actuators); + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } if (_actuators_1_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, - &_actuators_airframe); + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); // warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", // (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], // (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], @@ -978,8 +967,7 @@ void FixedwingAttitudeControl::task_main() { } else { /* advertise and publish */ - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), - &_actuators_airframe); + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); } } @@ -995,14 +983,18 @@ void FixedwingAttitudeControl::task_main() { _exit(0); } -int FixedwingAttitudeControl::start() { +int +FixedwingAttitudeControl::start() +{ ASSERT(_control_task == -1); /* start the task */ _control_task = task_spawn_cmd("fw_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, 2048, - (main_t) &FixedwingAttitudeControl::task_main_trampoline, nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&FixedwingAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -1012,7 +1004,8 @@ int FixedwingAttitudeControl::start() { return OK; } -int fw_att_control_main(int argc, char *argv[]) { +int fw_att_control_main(int argc, char *argv[]) +{ if (argc < 1) errx(1, "usage: fw_att_control {start|stop|status}"); @@ -1033,8 +1026,7 @@ int fw_att_control_main(int argc, char *argv[]) { } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - while (att_control::g_control == nullptr - || !att_control::g_control->task_running()) { + while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { usleep(50000); printf("."); fflush(stdout); -- cgit v1.2.3 From a0e2e4e8b30ef64728ff57e56a28b95527f4a1b0 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:01:47 +0100 Subject: pixhawk specific files --- src/modules/fw_att_control/fw_att_control_base.cpp | 1 - src/modules/fw_att_control/fw_att_control_base.h | 2 -- src/platforms/px4_defines.h | 5 ----- 3 files changed, 8 deletions(-) (limited to 'src/modules/fw_att_control') diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index d8ba15969..99780bc7e 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -56,7 +56,6 @@ FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : _setpoint_valid(false), _debug(false) { /* safely initialize structs */ _att = {}; - _accel = {}; _att_sp = {}; _manual = {}; _airspeed = {}; diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 6b2efc46b..1726c2e3e 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -54,7 +54,6 @@ #include #include -#include #include class FixedwingAttitudeControlBase @@ -78,7 +77,6 @@ protected: int _control_task; /**< task handle for sensor task */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 327d0bea1..1ff46b97c 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -48,11 +48,6 @@ #else #include //#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } - -#include "drv_sensor.h" -#include "drv_orb_dev.h" -#define ACCEL_DEVICE_PATH "/dev/accel" - #include #endif -- cgit v1.2.3 From 44127363b11a9280de818ff28047d170c487af1f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 12 Nov 2014 16:01:36 +0100 Subject: added setter and getter functions for use with euroc-gazebo simulator --- src/modules/fw_att_control/fw_att_control_base.cpp | 57 ++++++++++++++++++++++ src/modules/fw_att_control/fw_att_control_base.h | 6 +++ 2 files changed, 63 insertions(+) (limited to 'src/modules/fw_att_control') diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 99780bc7e..46fef3e67 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -267,3 +267,60 @@ void FixedwingAttitudeControlBase::control_attitude() { } } + +void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion 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 index 1726c2e3e..bde1b755f 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -140,6 +140,12 @@ protected: void control_attitude(); + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion 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_ */ -- cgit v1.2.3