fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 11:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control: inline control attitude call and rate publication

Browse Source 
- this makes it clear that we didn't need to keep local copies of the rate setpoint
This commit is contained in:
Daniel Agar
2020-08-10 14:07:42 -04:00
parent 14f734ac15
commit 5e739f66d6
2 changed files with 16 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control.hpp
-9
View File
@@ -89,8 +89,6 @@ private:
*/
void parameters_updated();
void publish_rates_setpoint();
float throttle_curve(float throttle_stick_input);
/**
@@ -98,11 +96,6 @@ private:
*/
void generate_attitude_setpoint(float dt, bool reset_yaw_sp);
/**
* Attitude controller.
*/
void control_attitude();
AttitudeControl _attitude_control; ///< class for attitude control calculations
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
@@ -119,7 +112,6 @@ private:
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub;
struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */
struct vehicle_rates_setpoint_s _v_rates_sp {}; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */
struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */
struct vehicle_status_s _vehicle_status {}; /**< vehicle status */
@@ -128,7 +120,6 @@ private:
perf_counter_t _loop_perf; /**< loop duration performance counter */
matrix::Vector3f _thrust_setpoint_body; ///< body frame 3D thrust vector
matrix::Vector3f _rates_sp; ///< angular rates setpoint
float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */
float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */
src/modules/mc_att_control/mc_att_control_main.cpp
+16 -30
View File
@@ -220,31 +220,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
}
/**
* Attitude controller.
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
* Output: '_rates_sp' vector
*/
void
MulticopterAttitudeControl::control_attitude()
{
_rates_sp = _attitude_control.update(Quatf(_v_att.q));
}
void
MulticopterAttitudeControl::publish_rates_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();
_v_rates_sp_pub.publish(v_rates_sp);
}
void
MulticopterAttitudeControl::Run()
{
@@ -333,16 +308,27 @@ MulticopterAttitudeControl::Run()
_man_y_input_filter.reset(0.f);
}
control_attitude();
Vector3f rates_sp = _attitude_control.update(Quatf(_v_att.q));
if (_v_control_mode.flag_control_yawrate_override_enabled) {
/* Yaw rate override enabled, overwrite the yaw setpoint */
_v_rates_sp_sub.update(&_v_rates_sp);
const auto yawrate_reference = _v_rates_sp.yaw;
_rates_sp(2) = yawrate_reference;
vehicle_rates_setpoint_s v_rates_sp{};
if (_v_rates_sp_sub.copy(&v_rates_sp)) {
const auto yawrate_reference = v_rates_sp.yaw;
rates_sp(2) = yawrate_reference;
}
}
publish_rates_setpoint();
// 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();
_v_rates_sp_pub.publish(v_rates_sp);
}
// reset yaw setpoint during transitions, tailsitter.cpp generates