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

mc_rate_control: use vehicle_angular_acceleration topic instead of differentiating

Browse Source 
* mc_rate_control: use vehicle_angular_acceleration topic
* IMU_DGYRO_CUTOFF change default to 30 Hz
* improve IMU_DGYRO_CUTOFF param documentation (updated from MC_DTERM_CUTOFF)
This commit is contained in:
Daniel Agar
2020-03-12 15:07:03 -04:00
committed by GitHub
parent 2b03aa3d1b
commit 093e9ba1ce
19 changed files with 56 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_rate_control/MulticopterRateControl.hpp
+10 -17
View File
@@ -35,7 +35,6 @@
#include <RateControl.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
@@ -54,6 +53,7 @@
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
@@ -93,15 +93,16 @@ private:
RateControl _rate_control; ///< class for rate control calculations
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
@@ -123,19 +124,13 @@ private:
perf_counter_t _loop_perf; /**< loop duration performance counter */
static constexpr const float initial_update_rate_hz = 1000.f; /**< loop update rate used for initialization */
float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */
matrix::Vector3f _rates_sp; /**< angular rates setpoint */
float _thrust_sp{0.0f}; /**< thrust setpoint */
bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */
hrt_abstime _task_start{hrt_absolute_time()};
hrt_abstime _last_run{0};
float _dt_accumulator{0.0f};
int _loop_counter{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
@@ -159,8 +154,6 @@ private:
(ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
(ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
(ParamFloat<px4::params::MC_DTERM_CUTOFF>) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,