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

generic position controller status/feedback message

Browse Source
This commit is contained in:
Daniel Agar
2018-08-31 14:43:51 -04:00
parent 223dacee64
commit 5207c420c3
16 changed files with 303 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+33 -29
View File
@@ -55,22 +55,23 @@
#include <cfloat>
#include <drivers/drv_hrt.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <ecl/tecs/tecs.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <lib/ecl/l1/ecl_l1_pos_controller.h>
#include <lib/ecl/tecs/tecs.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <perf/perf_counter.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_landing_status.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_bias.h>
@@ -86,23 +87,6 @@
#include <uORB/uORB.h>
#include <vtol_att_control/vtol_type.h>
static constexpr float HDG_HOLD_DIST_NEXT =
3000.0f; // initial distance of waypoint in front of plane in heading hold mode
static constexpr float HDG_HOLD_REACHED_DIST =
1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane
static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f; // max yawrate at which plane locks yaw for heading hold mode
static constexpr float HDG_HOLD_MAN_INPUT_THRESH =
0.01f; // max manual roll/yaw input from user which does not change the locked heading
static constexpr hrt_abstime T_ALT_TIMEOUT = 1; // time after which we abort landing if terrain estimate is not valid
static constexpr float THROTTLE_THRESH =
0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH =
0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode
static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
using math::constrain;
using math::max;
using math::min;
@@ -118,6 +102,24 @@ using uORB::Subscription;
using namespace launchdetection;
using namespace runwaytakeoff;
using namespace time_literals;
static constexpr float HDG_HOLD_DIST_NEXT =
3000.0f; // initial distance of waypoint in front of plane in heading hold mode
static constexpr float HDG_HOLD_REACHED_DIST =
1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane
static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f; // max yawrate at which plane locks yaw for heading hold mode
static constexpr float HDG_HOLD_MAN_INPUT_THRESH =
0.01f; // max manual roll/yaw input from user which does not change the locked heading
static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort landing if terrain estimate is not valid
static constexpr float THROTTLE_THRESH =
0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH =
0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode
static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams
{
@@ -161,12 +163,12 @@ private:
int _sensor_baro_sub{-1};
orb_advert_t _attitude_sp_pub{nullptr}; ///< attitude setpoint */
orb_advert_t _pos_ctrl_status_pub{nullptr}; ///< navigation capabilities publication */
orb_advert_t _pos_ctrl_landing_status_pub{nullptr}; ///< landing status publication */
orb_advert_t _tecs_status_pub{nullptr}; ///< TECS status publication */
orb_advert_t _fw_pos_ctrl_status_pub{nullptr}; ///< navigation capabilities publication */
orb_id_t _attitude_setpoint_id{nullptr};
fw_pos_ctrl_status_s _fw_pos_ctrl_status {}; ///< navigation capabilities */
manual_control_setpoint_s _manual {}; ///< r/c channel data */
position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items */
vehicle_attitude_s _att {}; ///< vehicle attitude setpoint */
@@ -202,6 +204,7 @@ private:
bool _land_stayonground{false};
bool _land_motor_lim{false};
bool _land_onslope{false};
bool _land_abort{false};
Landingslope _landingslope;
@@ -367,9 +370,7 @@ private:
} _parameter_handles {}; ///< handles for interesting parameters */
/**
* Update our local parameter cache.
*/
// Update our local parameter cache.
int parameters_update();
// Update subscriptions
@@ -383,8 +384,11 @@ private:
void vehicle_land_detected_poll();
void vehicle_status_poll();
// publish navigation capabilities
void fw_pos_ctrl_status_publish();
void status_publish();
void landing_status_publish();
void tecs_status_publish();
void abort_landing(bool abort);
/**
* Get a new waypoint based on heading and distance from current position