fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 13:54:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control: keep integral enabled based on land detector

Browse Source 
Previously the rate controller disabled updating the integral below 20%
throttle. This is not ideal for several reasons:
- some racers already hover with 20% throttle.
- for acro it is important to always keep the integral enabled, it has a
  noticeable effect on flight performance.
This commit is contained in:
Beat Küng 2018-06-16 12:09:59 +02:00 committed by Lorenz Meier
parent bf1c11a33c
commit bb8e653469
2 changed files with 22 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -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{};

21
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -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[])