diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index aeb782c881..af04bead61 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -407,6 +407,12 @@ 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) { + return; + } + + matrix::Dcmf R(matrix::Quatf(_v_att->q)); matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d)); matrix::Eulerf euler(R); 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 1694d41842..a1fe32c0ad 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -76,6 +76,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _manual_control_sp_sub(-1), _armed_sub(-1), _local_pos_sub(-1), + _local_pos_sp_sub(-1), _airspeed_sub(-1), _battery_status_sub(-1), _vehicle_cmd_sub(-1), @@ -109,6 +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(&_airspeed, 0, sizeof(_airspeed)); memset(&_batt_status, 0, sizeof(_batt_status)); memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); @@ -373,6 +375,22 @@ VtolAttitudeControl::vehicle_local_pos_poll() } +/** +* Check for position setpoint updates. +*/ +void +VtolAttitudeControl::vehicle_local_pos_sp_poll() +{ + bool updated; + /* Check if parameters have changed */ + orb_check(_local_pos_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp); + } + +} + /** * Check for mc virtual attitude setpoint updates. */ @@ -650,6 +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)); _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)); @@ -738,6 +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(); 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 5862317cce..95c34296ff 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -77,6 +77,7 @@ #include #include #include +#include #include #include #include @@ -116,6 +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 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;} @@ -142,6 +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 _airspeed_sub; // airspeed subscription int _battery_status_sub; // battery status subscription int _vehicle_cmd_sub; @@ -175,6 +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 airspeed_s _airspeed; // airspeed struct battery_status_s _batt_status; // battery status struct vehicle_command_s _vehicle_cmd; @@ -226,6 +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 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.