fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 07:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Standardize variable naming and formatting across vehicle attitude controller files.

Browse Source
This commit is contained in:
mcsauder
2022-05-21 12:22:18 -04:00
committed by Daniel Agar
parent 422be90140
commit 6b0788ff46
12 changed files with 240 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control.hpp
+33 -39
View File
@@ -60,11 +60,6 @@
using namespace time_literals;
/**
* Multicopter attitude control app start / stop handling function
*/
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>, public ModuleParams,
public px4::WorkItem
{
@@ -89,40 +84,41 @@ private:
/**
* initialize some vectors/matrices from parameters
*/
void parameters_updated();
void parameters_updated();
float throttle_curve(float throttle_stick_input);
float throttle_curve(float throttle_stick_input);
/**
* Generate & publish an attitude setpoint from stick inputs
*/
void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp);
void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp);
AttitudeControl _attitude_control; ///< class for attitude control calculations
AttitudeControl _attitude_control; /**< class for attitude control calculations */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub;
uORB::Publication<vehicle_rates_setpoint_s> _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub;
struct manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */
struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */
manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */
vehicle_control_mode_s _vehicle_control_mode {}; /**< vehicle control mode */
perf_counter_t _loop_perf; /**< loop duration performance counter */
perf_counter_t _loop_perf; /**< loop duration performance counter */
matrix::Vector3f _thrust_setpoint_body; ///< body frame 3D thrust vector
matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */
float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */
float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */
float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */
float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */
AlphaFilter<float> _man_x_input_filter;
AlphaFilter<float> _man_y_input_filter;
@@ -139,27 +135,25 @@ private:
uint8_t _quat_reset_counter{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MC_ROLL_P>) _param_mc_roll_p,
(ParamFloat<px4::params::MC_PITCH_P>) _param_mc_pitch_p,
(ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p,
(ParamFloat<px4::params::MC_YAW_WEIGHT>) _param_mc_yaw_weight,
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
(ParamFloat<px4::params::MC_MAN_TILT_TAU>) _param_mc_man_tilt_tau,
(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
(ParamFloat<px4::params::MC_ROLL_P>) _param_mc_roll_p,
(ParamFloat<px4::params::MC_PITCH_P>) _param_mc_pitch_p,
(ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p,
(ParamFloat<px4::params::MC_YAW_WEIGHT>) _param_mc_yaw_weight,
(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _param_mc_pitchrate_max,
(ParamFloat<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,
/* Stabilized mode params */
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min, /**< minimum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max, /**< maximum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_HOVER>)
_param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve, /**< throttle curve behavior */
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
(ParamFloat<px4::params::MC_MAN_TILT_TAU>) _param_mc_man_tilt_tau
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min, /**< minimum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max, /**< maximum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover, /**< throttle at stationary hover */
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve /**< throttle curve behavior */
)
};
src/modules/mc_att_control/mc_att_control_main.cpp
+18 -14
View File
@@ -277,7 +277,7 @@ MulticopterAttitudeControl::Run()
/* check for updates in other topics */
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_v_control_mode_sub.update(&_v_control_mode);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected;
@@ -306,15 +306,15 @@ MulticopterAttitudeControl::Run()
// vehicle is a tailsitter in transition mode
const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode);
bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
if (run_att_ctrl) {
// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
if (_v_control_mode.flag_control_manual_enabled &&
!_v_control_mode.flag_control_altitude_enabled &&
!_v_control_mode.flag_control_velocity_enabled &&
!_v_control_mode.flag_control_position_enabled) {
if (_vehicle_control_mode.flag_control_manual_enabled &&
!_vehicle_control_mode.flag_control_altitude_enabled &&
!_vehicle_control_mode.flag_control_velocity_enabled &&
!_vehicle_control_mode.flag_control_position_enabled) {
generate_attitude_setpoint(q, dt, _reset_yaw_sp);
attitude_setpoint_generated = true;
@@ -340,14 +340,14 @@ MulticopterAttitudeControl::Run()
}
// publish rate setpoint
vehicle_rates_setpoint_s v_rates_sp{};
v_rates_sp.roll = rates_sp(0);
v_rates_sp.pitch = rates_sp(1);
v_rates_sp.yaw = rates_sp(2);
_thrust_setpoint_body.copyTo(v_rates_sp.thrust_body);
v_rates_sp.timestamp = hrt_absolute_time();
vehicle_rates_setpoint_s rates_setpoint{};
rates_setpoint.roll = rates_sp(0);
rates_setpoint.pitch = rates_sp(1);
rates_setpoint.yaw = rates_sp(2);
_thrust_setpoint_body.copyTo(rates_setpoint.thrust_body);
rates_setpoint.timestamp = hrt_absolute_time();
_v_rates_sp_pub.publish(v_rates_sp);
_vehicle_rates_setpoint_pub.publish(rates_setpoint);
}
// reset yaw setpoint during transitions, tailsitter.cpp generates
@@ -425,7 +425,11 @@ https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth
return 0;
}
int mc_att_control_main(int argc, char *argv[])
/**
* Multicopter attitude control app start / stop handling function
*/
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
{
return MulticopterAttitudeControl::main(argc, argv);
}