dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: uorb home_postition subscription

This commit is contained in:
Dennis Mannhart 2017-02-23 13:36:24 +01:00 committed by Lorenz Meier
parent 3bc8617224
commit de9d05c292
1 changed files with 13 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -76,6 +76,7 @@
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/home_position.h>
#include <float.h>
#include <systemlib/mavlink_log.h>
@ -140,6 +141,7 @@ private:
int _pos_sp_triplet_sub; /**< position setpoint triplet */
int _local_pos_sp_sub; /**< offboard local position setpoint */
int _global_vel_sp_sub; /**< offboard global velocity setpoint */
int _home_pos_sub; /**< home position */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
@ -157,6 +159,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 vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
struct home_position_s _home_pos; /**< home position */
control::BlockParamFloat _manual_thr_min;
control::BlockParamFloat _manual_thr_max;
@ -405,6 +408,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_local_pos_sub(-1),
_pos_sp_triplet_sub(-1),
_global_vel_sp_sub(-1),
_home_pos_sub(-1),
/* publications */
_att_sp_pub(nullptr),
@ -422,6 +426,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_pos_sp_triplet{},
_local_pos_sp{},
_global_vel_sp{},
_home_pos{},
_manual_thr_min(this, "MANTHR_MIN"),
_manual_thr_max(this, "MANTHR_MAX"),
_manual_land_alt(this, "MIS_LTRMIN_ALT", false),
@ -808,6 +813,12 @@ MulticopterPositionControl::poll_subscriptions()
_pos_sp_triplet.current.valid = false;
}
}
orb_check(_home_pos_sub, &updated);
if (updated) {
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
}
}
float
@ -1685,7 +1696,7 @@ MulticopterPositionControl::control_position(float dt)
* for now we use the home altitude and assume that our Z coordinate
* is initialized close to home.
*/
bool close_to_ground = -_pos(2) < _manual_land_alt.get();
bool close_to_ground = (-_pos(2) - _home_pos.z) < _manual_land_alt.get();
if (close_to_ground && (_vel_sp(2) > _params.land_speed)) {
_vel_sp(2) = _params.land_speed;
@ -2219,6 +2230,7 @@ MulticopterPositionControl::task_main()
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
parameters_update(true);