From 33e67fd5276e872fa33be1bb4d5aff6a26567e5c Mon Sep 17 00:00:00 2001 From: sanderux Date: Tue, 15 Aug 2017 00:13:44 +0200 Subject: [PATCH] Disable PA during landing --- src/modules/vtol_att_control/standard.cpp | 5 +++-- .../vtol_att_control/vtol_att_control_main.cpp | 14 +++++++------- .../vtol_att_control/vtol_att_control_main.h | 10 +++++----- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index af04bead61..29a7ba99ce 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -407,8 +407,9 @@ void Standard::update_mc_state() return; } - // disable pusher assist when descending - if (PX4_ISFINITE(_attc->get_local_pos_sp()->vz) && _attc->get_local_pos_sp()->vz > 0.5f) { + // disable pusher assist during landing + if (_attc->get_pos_sp_triplet()->current.valid + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { return; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index a1fe32c0ad..e8df32cce6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -76,7 +76,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _manual_control_sp_sub(-1), _armed_sub(-1), _local_pos_sub(-1), - _local_pos_sp_sub(-1), + _pos_sp_triplet_sub(-1), _airspeed_sub(-1), _battery_status_sub(-1), _vehicle_cmd_sub(-1), @@ -110,7 +110,7 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); memset(&_local_pos, 0, sizeof(_local_pos)); - memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); + memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_airspeed, 0, sizeof(_airspeed)); memset(&_batt_status, 0, sizeof(_batt_status)); memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); @@ -379,14 +379,14 @@ VtolAttitudeControl::vehicle_local_pos_poll() * Check for position setpoint updates. */ void -VtolAttitudeControl::vehicle_local_pos_sp_poll() +VtolAttitudeControl::pos_sp_triplet_poll() { bool updated; /* Check if parameters have changed */ - orb_check(_local_pos_sp_sub, &updated); + orb_check(_pos_sp_triplet_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp); + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); } } @@ -668,7 +668,7 @@ void VtolAttitudeControl::task_main() _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -757,7 +757,7 @@ void VtolAttitudeControl::task_main() vehicle_rates_sp_fw_poll(); parameters_update_poll(); vehicle_local_pos_poll(); // Check for new sensor values - vehicle_local_pos_sp_poll(); + pos_sp_triplet_poll(); vehicle_airspeed_poll(); vehicle_battery_poll(); vehicle_cmd_poll(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 95c34296ff..2219a80a76 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -77,7 +77,7 @@ #include #include #include -#include +#include #include #include #include @@ -117,7 +117,7 @@ public: struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} struct actuator_armed_s *get_armed() {return &_armed;} struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} - struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;} + struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;} struct airspeed_s *get_airspeed() {return &_airspeed;} struct battery_status_s *get_batt_status() {return &_batt_status;} struct tecs_status_s *get_tecs_status() {return &_tecs_status;} @@ -144,7 +144,7 @@ private: int _manual_control_sp_sub; //manual control setpoint subscription int _armed_sub; //arming status subscription int _local_pos_sub; // sensor subscription - int _local_pos_sp_sub; // local position setpoint subscription + int _pos_sp_triplet_sub; // local position setpoint subscription int _airspeed_sub; // airspeed subscription int _battery_status_sub; // battery status subscription int _vehicle_cmd_sub; @@ -178,7 +178,7 @@ private: struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control struct actuator_armed_s _armed; //actuator arming status struct vehicle_local_position_s _local_pos; - struct vehicle_local_position_setpoint_s _local_pos_sp; + struct position_setpoint_triplet_s _pos_sp_triplet; struct airspeed_s _airspeed; // airspeed struct battery_status_s _batt_status; // battery status struct vehicle_command_s _vehicle_cmd; @@ -230,7 +230,7 @@ private: void vehicle_rates_sp_mc_poll(); void vehicle_rates_sp_fw_poll(); void vehicle_local_pos_poll(); // Check for changes in sensor values - void vehicle_local_pos_sp_poll(); // Check for changes in position setpoint values + void pos_sp_triplet_poll(); // Check for changes in position setpoint values void vehicle_airspeed_poll(); // Check for changes in airspeed void vehicle_attitude_setpoint_poll(); //Check for attitude setpoint updates. void vehicle_attitude_poll(); //Check for attitude updates.