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

mc_pos_control: subscribe to trajectory_waypoint message

Browse Source
This commit is contained in:
Martina
2018-04-05 14:52:49 +02:00
committed by Daniel Agar
parent f3ce7be1e3
commit 14444af38c
1 changed files with 12 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+12
View File
@@ -59,6 +59,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/trajectory_waypoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -148,6 +149,7 @@ private:
int _local_pos_sub; /**< vehicle local position */
int _pos_sp_triplet_sub; /**< position setpoint triplet */
int _home_pos_sub; /**< home position */
int _traj_wp_avoidance_sub; /**< trajectory waypoint */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
@@ -164,6 +166,7 @@ private:
struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */
struct home_position_s _home_pos; /**< home position */
struct trajectory_waypoint_s _traj_wp_avoidance; /**< trajectory waypoint */
DEFINE_PARAMETERS(
(ParamInt<px4::params::MPC_FLT_TSK>) _test_flight_tasks, /**< temporary flag for the transition to flight tasks */
@@ -440,6 +443,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_local_pos_sub(-1),
_pos_sp_triplet_sub(-1),
_home_pos_sub(-1),
_traj_wp_avoidance_sub(-1),
/* publications */
_att_sp_pub(nullptr),
@@ -455,6 +459,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_pos_sp_triplet{},
_local_pos_sp{},
_home_pos{},
_traj_wp_avoidance{},
_vel_x_deriv(this, "VELD"),
_vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD"),
@@ -745,6 +750,12 @@ MulticopterPositionControl::poll_subscriptions()
if (updated) {
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
}
orb_check(_traj_wp_avoidance_sub, &updated);
if (updated) {
orb_copy(ORB_ID(trajectory_waypoint), _traj_wp_avoidance_sub, &_traj_wp_avoidance);
}
}
float
@@ -2929,6 +2940,7 @@ MulticopterPositionControl::task_main()
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
_traj_wp_avoidance_sub = orb_subscribe(ORB_ID(trajectory_waypoint));
parameters_update(true);