From f947205cbebb225e1c32d766ea37b3bd2e1bf1f5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 24 Jun 2017 20:08:31 -0400 Subject: [PATCH] delete vehicle_global_velocity_setpoint --- msg/CMakeLists.txt | 1 - msg/vehicle_global_velocity_setpoint.msg | 4 ---- .../mc_pos_control.cpp | 16 ---------------- .../mc_pos_control.h | 5 ----- src/modules/logger/logger.cpp | 1 - src/modules/mavlink/mavlink_receiver.h | 1 - .../mc_pos_control/mc_pos_control_main.cpp | 18 ------------------ src/modules/sdlog2/sdlog2.c | 13 ------------- src/platforms/px4_includes.h | 4 ---- 9 files changed, 63 deletions(-) delete mode 100644 msg/vehicle_global_velocity_setpoint.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 4b0181516d..7fc5e6ab91 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -115,7 +115,6 @@ set(msg_file_names vehicle_control_mode.msg vehicle_force_setpoint.msg vehicle_global_position.msg - vehicle_global_velocity_setpoint.msg vehicle_gps_position.msg vehicle_land_detected.msg vehicle_local_position.msg diff --git a/msg/vehicle_global_velocity_setpoint.msg b/msg/vehicle_global_velocity_setpoint.msg deleted file mode 100644 index ca7dcc826a..0000000000 --- a/msg/vehicle_global_velocity_setpoint.msg +++ /dev/null @@ -1,4 +0,0 @@ -# Velocity setpoint in NED frame -float32 vx # in m/s NED -float32 vy # in m/s NED -float32 vz # in m/s NED diff --git a/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp index 1563dc7b1c..5ed52a4b27 100644 --- a/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -58,12 +58,10 @@ MulticopterPositionControlMultiplatform::MulticopterPositionControlMultiplatform /* publications */ _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), - _global_vel_sp_pub(nullptr), /* outgoing messages */ _att_sp_msg(), _local_pos_sp_msg(), - _global_vel_sp_msg(), _n(_appState), @@ -116,8 +114,6 @@ _R() _pos_sp_triplet = _n.subscribe (&MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet, this, 0); _local_pos_sp = _n.subscribe(0); - _global_vel_sp = _n.subscribe(0); - _params.pos_p.zero(); _params.vel_p.zero(); @@ -700,18 +696,6 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4 _vel_sp(2) = _params.land_speed; } - _global_vel_sp_msg.data().vx = _vel_sp(0); - _global_vel_sp_msg.data().vy = _vel_sp(1); - _global_vel_sp_msg.data().vz = _vel_sp(2); - - /* publish velocity setpoint */ - if (_global_vel_sp_pub != nullptr) { - _global_vel_sp_pub->publish(_global_vel_sp_msg); - - } else { - _global_vel_sp_pub = _n.advertise(); - } - if (_control_mode->data().flag_control_climb_rate_enabled || _control_mode->data().flag_control_velocity_enabled) { /* reset integrals if needed */ if (_control_mode->data().flag_control_climb_rate_enabled) { diff --git a/src/examples/mc_pos_control_multiplatform/mc_pos_control.h b/src/examples/mc_pos_control_multiplatform/mc_pos_control.h index ab44209a7f..d282e7acd2 100644 --- a/src/examples/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/examples/mc_pos_control_multiplatform/mc_pos_control.h @@ -88,9 +88,6 @@ protected: Publisher *_att_sp_pub; /**< attitude setpoint publication */ Publisher *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ - Publisher - *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ - Subscriber *_att; /**< vehicle attitude */ Subscriber *_control_mode; /**< vehicle control mode */ @@ -100,11 +97,9 @@ protected: Subscriber *_local_pos; /**< local position */ Subscriber *_pos_sp_triplet; /**< local position */ Subscriber *_local_pos_sp; /**< local position */ - Subscriber *_global_vel_sp; /**< local position */ px4_vehicle_attitude_setpoint _att_sp_msg; px4_vehicle_local_position_setpoint _local_pos_sp_msg; - px4_vehicle_global_velocity_setpoint _global_vel_sp_msg; px4::NodeHandle _n; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 07bf48282b..eae5ed68a9 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -576,7 +576,6 @@ void Logger::add_default_topics() add_topic("vehicle_local_position", 100); add_topic("vehicle_local_position_setpoint", 100); add_topic("vehicle_global_position", 200); - add_topic("vehicle_global_velocity_setpoint", 200); add_topic("vehicle_vision_position"); add_topic("vehicle_vision_attitude"); add_topic("battery_status", 300); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4270392775..51fa8189d0 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -56,7 +56,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 29de6bea8b..befb2e00a8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -61,7 +61,6 @@ #include #include #include -#include #include #include #include @@ -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: 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() : /* 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() : _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) /* 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 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 4296422fa8..ea3b335527 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -79,7 +79,6 @@ #include #include #include -#include #include #include #include @@ -1186,7 +1185,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct differential_pressure_s diff_pres; struct airspeed_s airspeed; struct esc_status_s esc; - struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; struct telemetry_status_s telemetry; struct distance_sensor_s distance_sensor; @@ -1302,7 +1300,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int rc_sub; int airspeed_sub; int esc_sub; - int global_vel_sp_sub; int battery_sub; int telemetry_subs[ORB_MULTI_MAX_INSTANCES]; int distance_sensor_sub; @@ -1348,7 +1345,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.rc_sub = -1; subs.airspeed_sub = -1; subs.esc_sub = -1; - subs.global_vel_sp_sub = -1; subs.battery_sub = -1; subs.distance_sensor_sub = -1; subs.estimator_status_sub = -1; @@ -2069,15 +2065,6 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- GLOBAL VELOCITY SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), &subs.global_vel_sp_sub, &buf.global_vel_sp)) { - log_msg.msg_type = LOG_GVSP_MSG; - log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; - log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; - log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; - LOGBUFFER_WRITE_AND_COUNT(GVSP); - } - /* --- BATTERY --- */ if (copy_if_updated(ORB_ID(battery_status), &subs.battery_sub, &buf.battery)) { log_msg.msg_type = LOG_BATT_MSG; diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 434bd5e78b..4de88db471 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -62,7 +62,6 @@ #include #include #include -#include #include #include #include @@ -88,7 +87,6 @@ #include #include #include -#include #include #include #include @@ -119,7 +117,6 @@ #include #include #include -#include #include #include #endif @@ -146,7 +143,6 @@ #include #include #include -#include #include #include #endif