From 426f2cce2e54bc40c4ca74cf0385ee199493cd5e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 16 Feb 2018 18:17:16 -0500 Subject: [PATCH] fw_att_control split header into separate file --- .../FixedwingAttitudeControl.hpp | 308 ++++++++++++++++++ .../fw_att_control/fw_att_control_main.cpp | 287 +--------------- 2 files changed, 309 insertions(+), 286 deletions(-) create mode 100644 src/modules/fw_att_control/FixedwingAttitudeControl.hpp diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp new file mode 100644 index 0000000000..beba790def --- /dev/null +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -0,0 +1,308 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2017 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Eulerf; +using matrix::Quatf; + +using uORB::Subscription; + +class FixedwingAttitudeControl final : public control::SuperBlock, public ModuleBase +{ +public: + FixedwingAttitudeControl(); + ~FixedwingAttitudeControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static FixedwingAttitudeControl *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + + int _att_sub; /**< vehicle attitude */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _battery_status_sub; /**< battery status subscription */ + int _global_pos_sub; /**< global position subscription */ + int _manual_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _vehicle_land_detected_sub; /**< vehicle land detected 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_2_pub; /**< actuator control group 1 setpoint (Airframe) */ + orb_advert_t _rate_ctrl_status_pub; /**< rate controller status 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 + orb_id_t _attitude_setpoint_id; + + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct battery_status_s _battery_status; /**< battery status */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_attitude_s _att; /**< vehicle attitude setpoint */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_land_detected_s _vehicle_land_detected; /**< vehicle land detected */ + struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + + Subscription _sub_airspeed; + + 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 */ + + float _flaps_applied; + float _flaperons_applied; + + struct { + float p_tc; + float p_p; + float p_i; + float p_ff; + float p_rmax_pos; + float p_rmax_neg; + float p_integrator_max; + float r_tc; + float r_p; + float r_i; + float r_ff; + float r_integrator_max; + float r_rmax; + float y_p; + float y_i; + float y_ff; + float y_integrator_max; + float roll_to_yaw_ff; + float y_rmax; + + bool w_en; + float w_p; + float w_i; + float w_ff; + float w_integrator_max; + float w_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 */ + float man_roll_scale; /**< scale factor applied to roll actuator control in pure manual mode */ + float man_pitch_scale; /**< scale factor applied to pitch actuator control in pure manual mode */ + float man_yaw_scale; /**< scale factor applied to yaw actuator control in pure manual mode */ + + float acro_max_x_rate_rad; + float acro_max_y_rate_rad; + float acro_max_z_rate_rad; + + float flaps_scale; /**< Scale factor for flaps */ + float flaperon_scale; /**< Scale factor for flaperons */ + + float rattitude_thres; + + int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ + + int32_t bat_scale_en; /**< Battery scaling enabled */ + + } _parameters{}; /**< local copies of interesting parameters */ + + struct { + + param_t p_tc; + param_t p_p; + param_t p_i; + param_t p_ff; + param_t p_rmax_pos; + param_t p_rmax_neg; + param_t p_integrator_max; + param_t r_tc; + param_t r_p; + param_t r_i; + param_t r_ff; + param_t r_integrator_max; + param_t r_rmax; + param_t y_p; + param_t y_i; + param_t y_ff; + param_t y_integrator_max; + param_t roll_to_yaw_ff; + param_t y_rmax; + + param_t w_en; + param_t w_p; + param_t w_i; + param_t w_ff; + param_t w_integrator_max; + param_t w_rmax; + + param_t airspeed_min; + param_t airspeed_trim; + param_t airspeed_max; + + param_t trim_roll; + param_t trim_pitch; + param_t trim_yaw; + param_t rollsp_offset_deg; + param_t pitchsp_offset_deg; + param_t man_roll_max; + param_t man_pitch_max; + param_t man_roll_scale; + param_t man_pitch_scale; + param_t man_yaw_scale; + + param_t acro_max_x_rate; + param_t acro_max_y_rate; + param_t acro_max_z_rate; + + param_t flaps_scale; + param_t flaperon_scale; + + param_t rattitude_thres; + + param_t vtol_type; + + param_t bat_scale_en; + + } _parameter_handles{}; /**< handles for interesting parameters */ + + // Rotation matrix and euler angles to extract from control state + math::Matrix<3, 3> _R; + float _roll{0.0f}; + float _pitch{0.0f}; + float _yaw{0.0f}; + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + ECL_WheelController _wheel_ctrl; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Check for global position updates. + */ + void global_pos_poll(); + + /** + * Check for vehicle status updates. + */ + void vehicle_status_poll(); + + /** + * Check for vehicle land detected updates. + */ + void vehicle_land_detected_poll(); + + /** + * Check for battery status updates. + */ + void battery_status_poll(); + +}; 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 9c1e780b51..6b3f3ac2a1 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -31,52 +31,7 @@ * ****************************************************************************/ -/** - * @file fw_att_control_main.c - * Implementation of a generic attitude controller based on classic orthogonal PIDs. - * - * @author Lorenz Meier - * @author Thomas Gubler - * @author Roman Bapst - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using matrix::Eulerf; -using matrix::Quatf; - -using uORB::Subscription; +#include "FixedwingAttitudeControl.hpp" /** * Fixedwing attitude control app start / stop handling function @@ -85,246 +40,6 @@ using uORB::Subscription; */ extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); - -class FixedwingAttitudeControl final : public control::SuperBlock, public ModuleBase -{ -public: - FixedwingAttitudeControl(); - ~FixedwingAttitudeControl() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static FixedwingAttitudeControl *instantiate(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - /** @see ModuleBase::run() */ - void run() override; - - /** @see ModuleBase::print_status() */ - int print_status() override; - -private: - - int _att_sub; /**< vehicle attitude */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _battery_status_sub; /**< battery status subscription */ - int _global_pos_sub; /**< global position subscription */ - int _manual_sub; /**< notification of manual control updates */ - int _params_sub; /**< notification of parameter updates */ - int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _vehicle_land_detected_sub; /**< vehicle land detected 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_2_pub; /**< actuator control group 1 setpoint (Airframe) */ - orb_advert_t _rate_ctrl_status_pub; /**< rate controller status 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 - orb_id_t _attitude_setpoint_id; - - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ - struct battery_status_s _battery_status; /**< battery status */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_attitude_s _att; /**< vehicle attitude setpoint */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct vehicle_global_position_s _global_pos; /**< global position */ - struct vehicle_land_detected_s _vehicle_land_detected; /**< vehicle land detected */ - struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */ - struct vehicle_status_s _vehicle_status; /**< vehicle status */ - - Subscription _sub_airspeed; - - 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 */ - - float _flaps_applied; - float _flaperons_applied; - - struct { - float p_tc; - float p_p; - float p_i; - float p_ff; - float p_rmax_pos; - float p_rmax_neg; - float p_integrator_max; - float r_tc; - float r_p; - float r_i; - float r_ff; - float r_integrator_max; - float r_rmax; - float y_p; - float y_i; - float y_ff; - float y_integrator_max; - float roll_to_yaw_ff; - float y_rmax; - - bool w_en; - float w_p; - float w_i; - float w_ff; - float w_integrator_max; - float w_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 */ - float man_roll_scale; /**< scale factor applied to roll actuator control in pure manual mode */ - float man_pitch_scale; /**< scale factor applied to pitch actuator control in pure manual mode */ - float man_yaw_scale; /**< scale factor applied to yaw actuator control in pure manual mode */ - - float acro_max_x_rate_rad; - float acro_max_y_rate_rad; - float acro_max_z_rate_rad; - - float flaps_scale; /**< Scale factor for flaps */ - float flaperon_scale; /**< Scale factor for flaperons */ - - float rattitude_thres; - - int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ - - int32_t bat_scale_en; /**< Battery scaling enabled */ - - } _parameters{}; /**< local copies of interesting parameters */ - - struct { - - param_t p_tc; - param_t p_p; - param_t p_i; - param_t p_ff; - param_t p_rmax_pos; - param_t p_rmax_neg; - param_t p_integrator_max; - param_t r_tc; - param_t r_p; - param_t r_i; - param_t r_ff; - param_t r_integrator_max; - param_t r_rmax; - param_t y_p; - param_t y_i; - param_t y_ff; - param_t y_integrator_max; - param_t roll_to_yaw_ff; - param_t y_rmax; - - param_t w_en; - param_t w_p; - param_t w_i; - param_t w_ff; - param_t w_integrator_max; - param_t w_rmax; - - param_t airspeed_min; - param_t airspeed_trim; - param_t airspeed_max; - - param_t trim_roll; - param_t trim_pitch; - param_t trim_yaw; - param_t rollsp_offset_deg; - param_t pitchsp_offset_deg; - param_t man_roll_max; - param_t man_pitch_max; - param_t man_roll_scale; - param_t man_pitch_scale; - param_t man_yaw_scale; - - param_t acro_max_x_rate; - param_t acro_max_y_rate; - param_t acro_max_z_rate; - - param_t flaps_scale; - param_t flaperon_scale; - - param_t rattitude_thres; - - param_t vtol_type; - - param_t bat_scale_en; - - } _parameter_handles{}; /**< handles for interesting parameters */ - - // Rotation matrix and euler angles to extract from control state - math::Matrix<3, 3> _R; - float _roll{0.0f}; - float _pitch{0.0f}; - float _yaw{0.0f}; - - ECL_RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; - ECL_WheelController _wheel_ctrl; - - /** - * Update our local parameter cache. - */ - int parameters_update(); - - /** - * Check for changes in vehicle control mode. - */ - void vehicle_control_mode_poll(); - - /** - * Check for changes in manual inputs. - */ - void vehicle_manual_poll(); - - /** - * Check for set triplet updates. - */ - void vehicle_setpoint_poll(); - - /** - * Check for global position updates. - */ - void global_pos_poll(); - - /** - * Check for vehicle status updates. - */ - void vehicle_status_poll(); - - /** - * Check for vehicle land detected updates. - */ - void vehicle_land_detected_poll(); - - /** - * Check for battery status updates. - */ - void battery_status_poll(); - -}; - FixedwingAttitudeControl::FixedwingAttitudeControl() : SuperBlock(nullptr, "FW_ATT"), /* subscriptions */