|
|
|
@ -61,7 +61,6 @@
@@ -61,7 +61,6 @@
|
|
|
|
|
#include <uORB/topics/parameter_update.h> |
|
|
|
|
#include <uORB/topics/position_setpoint_triplet.h> |
|
|
|
|
#include <uORB/topics/vehicle_control_mode.h> |
|
|
|
|
#include <uORB/topics/vehicle_global_velocity_setpoint.h> |
|
|
|
|
#include <uORB/topics/vehicle_land_detected.h> |
|
|
|
|
#include <uORB/topics/vehicle_local_position.h> |
|
|
|
|
#include <uORB/topics/vehicle_local_position_setpoint.h> |
|
|
|
@ -121,7 +120,6 @@ private:
@@ -121,7 +120,6 @@ private:
|
|
|
|
|
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 */ |
|
|
|
|
|
|
|
|
|
orb_id_t _attitude_setpoint_id; |
|
|
|
|
|
|
|
|
@ -134,7 +132,6 @@ private:
@@ -134,7 +132,6 @@ private:
|
|
|
|
|
struct vehicle_local_position_s _local_pos; /**< vehicle local position */ |
|
|
|
|
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; /**< minimal throttle output when flying in manual mode */ |
|
|
|
@ -394,7 +391,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
@@ -394,7 +391,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
|
|
|
|
/* publications */ |
|
|
|
|
_att_sp_pub(nullptr), |
|
|
|
|
_local_pos_sp_pub(nullptr), |
|
|
|
|
_global_vel_sp_pub(nullptr), |
|
|
|
|
_attitude_setpoint_id(nullptr), |
|
|
|
|
_vehicle_status{}, |
|
|
|
|
_vehicle_land_detected{}, |
|
|
|
@ -405,7 +401,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
@@ -405,7 +401,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
|
|
|
|
_local_pos{}, |
|
|
|
|
_pos_sp_triplet{}, |
|
|
|
|
_local_pos_sp{}, |
|
|
|
|
_global_vel_sp{}, |
|
|
|
|
_home_pos{}, |
|
|
|
|
_manual_thr_min(this, "MANTHR_MIN"), |
|
|
|
|
_manual_thr_max(this, "MANTHR_MAX"), |
|
|
|
@ -1782,19 +1777,6 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
@@ -1782,19 +1777,6 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
|
|
|
|
|
/* limit vertical velocity to the current ramp value */ |
|
|
|
|
_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_vel_limit); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* publish velocity setpoint */ |
|
|
|
|
_global_vel_sp.timestamp = hrt_absolute_time(); |
|
|
|
|
_global_vel_sp.vx = _vel_sp(0); |
|
|
|
|
_global_vel_sp.vy = _vel_sp(1); |
|
|
|
|
_global_vel_sp.vz = _vel_sp(2); |
|
|
|
|
|
|
|
|
|
if (_global_vel_sp_pub != nullptr) { |
|
|
|
|
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|