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

fw_pos_control_l1: cleanup body velocity and acceleration usage

Browse Source 
- minimize storing unnecessary state (msg copies)
This commit is contained in:
Daniel Agar
2021-02-12 00:41:39 +01:00
committed by Lorenz Meier
parent 9ebf783214
commit cb7aabf932
4 changed files with 75 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+14 -19
View File
@@ -76,7 +76,6 @@
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -102,8 +101,6 @@ using matrix::Vector2f;
using matrix::Vector3f;
using matrix::wrap_pi;
using uORB::SubscriptionData;
using namespace launchdetection;
using namespace runwaytakeoff;
using namespace time_literals;
@@ -150,16 +147,17 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; ///< vehicle attitude subscription
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; ///< vehicle command subscription
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; ///< vehicle land detected subscription
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; ///< vehicle status subscription
uORB::SubscriptionData<vehicle_angular_velocity_s> _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication
@@ -168,16 +166,11 @@ private:
manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data
position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items
vehicle_attitude_s _att {}; ///< vehicle attitude setpoint
vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint
vehicle_control_mode_s _control_mode {}; ///< control mode
vehicle_local_position_s _local_pos {}; ///< vehicle local position
vehicle_land_detected_s _vehicle_land_detected {}; ///< vehicle land detected
vehicle_status_s _vehicle_status {}; ///< vehicle status
SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
SubscriptionData<vehicle_acceleration_s> _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
double _current_latitude{0};
double _current_longitude{0};
float _current_altitude{0.f};
@@ -199,6 +192,8 @@ private:
hrt_abstime _control_position_last_called{0}; ///< last call of control_position
bool _landed{true};
/* Landing */
bool _land_noreturn_horizontal{false};
bool _land_noreturn_vertical{false};
@@ -237,12 +232,12 @@ private:
float _airspeed{0.0f};
float _eas2tas{1.0f};
float _groundspeed_undershoot{0.0f}; ///< ground speed error to min. speed in m/s
Dcmf _R_nb; ///< current attitude
float _roll{0.0f};
float _pitch{0.0f};
float _yaw{0.0f};
float _yawrate{0.0f};
matrix::Vector3f _body_acceleration{};
matrix::Vector3f _body_velocity{};
bool _reinitialize_tecs{true}; ///< indicates if the TECS states should be reinitialized (used for VTOL)
bool _is_tecs_running{false};