diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 0ed06ee1a15746bca546e52a133b2612b6430d59..5f2f032b1fed75b97a7c8ae55b391d3db3ade4de 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -55,6 +55,7 @@ #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_land_detected.h> /** * Multicopter attitude control app start / stop handling function @@ -99,6 +100,7 @@ private: void battery_status_poll(); void parameter_update_poll(); void sensor_bias_poll(); + void vehicle_land_detected_poll(); void sensor_correction_poll(); void vehicle_attitude_poll(); void vehicle_attitude_setpoint_poll(); @@ -136,6 +138,7 @@ private: int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */ int _sensor_correction_sub{-1}; /**< sensor thermal correction subscription */ int _sensor_bias_sub{-1}; /**< sensor in-run bias correction subscription */ + int _vehicle_land_detected_sub{-1}; /**< vehicle land detected subscription */ unsigned _gyro_count{1}; int _selected_gyro{0}; @@ -160,6 +163,7 @@ private: struct sensor_gyro_s _sensor_gyro {}; /**< gyro data before thermal correctons and ekf bias estimates are applied */ struct sensor_correction_s _sensor_correction {}; /**< sensor thermal corrections */ struct sensor_bias_s _sensor_bias {}; /**< sensor in-run bias corrections */ + struct vehicle_land_detected_s _vehicle_land_detected {}; MultirotorMixer::saturation_status _saturation_status{}; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 170d6b3afe6fa1fd941b9b1dc3eaf71fbbc0b140..5626f96e021da807adef7b61566f1cbb212209ee 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -52,7 +52,6 @@ #include <mathlib/math/Limits.hpp> #include <mathlib/math/Functions.hpp> -#define MIN_TAKEOFF_THRUST 0.2f #define TPA_RATE_LOWER_LIMIT 0.05f #define AXIS_INDEX_ROLL 0 @@ -358,6 +357,19 @@ MulticopterAttitudeControl::sensor_bias_poll() } +void +MulticopterAttitudeControl::vehicle_land_detected_poll() +{ + /* check if there is a new message */ + bool updated; + orb_check(_vehicle_land_detected_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected); + } + +} + /** * Attitude controller. * Input: 'vehicle_attitude_setpoint' topics (depending on mode) @@ -541,8 +553,8 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_prev = rates; _rates_prev_filtered = rates_filtered; - /* update integral only if motors are providing enough thrust to be effective */ - if (_thrust_sp > MIN_TAKEOFF_THRUST) { + /* update integral only if we are not landed */ + if (!_vehicle_land_detected.maybe_landed && !_vehicle_land_detected.landed) { for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { // Check for positive control saturation bool positive_saturation = @@ -614,6 +626,7 @@ MulticopterAttitudeControl::run() _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); + _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); /* wakeup source: gyro data from sensor selected by the sensor app */ px4_pollfd_struct_t poll_fds = {}; @@ -673,6 +686,7 @@ MulticopterAttitudeControl::run() vehicle_attitude_poll(); sensor_correction_poll(); sensor_bias_poll(); + vehicle_land_detected_poll(); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't @@ -846,6 +860,7 @@ MulticopterAttitudeControl::run() orb_unsubscribe(_sensor_correction_sub); orb_unsubscribe(_sensor_bias_sub); + orb_unsubscribe(_vehicle_land_detected_sub); } int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])