Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Tradheli support for integrator management and hover collective learning

This commit is contained in:
bnsgeyer 2020-11-15 18:32:31 -05:00 committed by Bill Geyer
parent 42b4fd6e0a
commit 84fb3afbcb
5 changed files with 131 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -96,6 +96,20 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @Path: AP_MotorsHeli_RSC.cpp
AP_SUBGROUPINFO(_main_rotor, "RSC_", 25, AP_MotorsHeli, AP_MotorsHeli_RSC),
// @Param: COLL_HOVER
// @DisplayName: Collective Hover Value
// @Description: Collective needed to hover expressed as a number from 0 to 1 where 0 is H_COL_MIN and 1 is H_COL_MAX
// @Range: 0.3 0.8
// @User: Advanced
AP_GROUPINFO("COLL_HOVER", 26, AP_MotorsHeli, _collective_hover, AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT),
// @Param: HOVER_LEARN
// @DisplayName: Hover Value Learning
// @Description: Enable/Disable automatic learning of hover collective
// @Values: 0:Disabled, 1:Learn, 2:Learn and Save
// @User: Advanced
AP_GROUPINFO("HOVER_LEARN", 27, AP_MotorsHeli, _collective_hover_learn, HOVER_LEARN_AND_SAVE),
AP_GROUPEND
};
@ -301,6 +315,11 @@ void AP_MotorsHeli::output_logic()
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::SHUT_DOWN) {
_spool_state = SpoolState::GROUND_IDLE;
@ -311,6 +330,17 @@ void AP_MotorsHeli::output_logic()
case SpoolState::GROUND_IDLE: {
// Motors should be stationary or at ground idle.
// set limits flags
if (_heliflags.land_complete) {
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
} else {
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
}
// Servos should be moving to correct the current attitude.
if (_spool_desired == DesiredSpoolState::SHUT_DOWN){
_spool_state = SpoolState::SHUT_DOWN;
@ -326,6 +356,17 @@ void AP_MotorsHeli::output_logic()
// Maximum throttle should move from minimum to maximum.
// Servos should exhibit normal flight behavior.
// set limits flags
if (_heliflags.land_complete) {
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
} else {
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
}
// make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED ){
_spool_state = SpoolState::SPOOLING_DOWN;
@ -341,6 +382,17 @@ void AP_MotorsHeli::output_logic()
// Throttle should exhibit normal flight behavior.
// Servos should exhibit normal flight behavior.
// set limits flags
if (_heliflags.land_complete) {
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
} else {
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
}
// make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) {
_spool_state = SpoolState::SPOOLING_DOWN;
@ -354,6 +406,17 @@ void AP_MotorsHeli::output_logic()
// Maximum throttle should move from maximum to minimum.
// Servos should exhibit normal flight behavior.
// set limits flags
if (_heliflags.land_complete) {
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
} else {
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
}
// make sure the motors are spooling in the correct direction
if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) {
_spool_state = SpoolState::SPOOLING_UP;
@ -456,3 +519,27 @@ void AP_MotorsHeli::rc_write_swash(uint8_t chan, float swash_in)
SRV_Channels::set_output_pwm_trimmed(function, pwm);
}
// update the collective input filter. should be called at 100hz
void AP_MotorsHeli::update_throttle_hover(float dt)
{
if (_collective_hover_learn != HOVER_LEARN_DISABLED) {
// Don't let _collective_hover go below H_COLL_MID
float curr_collective = get_throttle();
if (curr_collective < _collective_mid_pct) {
curr_collective = _collective_mid_pct;
}
// we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial.
_collective_hover = constrain_float(_collective_hover + (dt / (dt + AP_MOTORS_HELI_COLLECTIVE_HOVER_TC)) * (curr_collective - _collective_hover), AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN, AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX);
}
}
// save parameters as part of disarming
void AP_MotorsHeli::save_params_on_disarm()
{
// save hover throttle
if (_collective_hover_learn == HOVER_LEARN_AND_SAVE) {
_collective_hover.save();
}
}

27
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -19,6 +19,10 @@
#define AP_MOTORS_HELI_COLLECTIVE_MIN 1250
#define AP_MOTORS_HELI_COLLECTIVE_MAX 1750
#define AP_MOTORS_HELI_COLLECTIVE_MID 1500
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT 0.5f // the estimated hover throttle, 0 ~ 1
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_TC 10.0f // time constant used to update estimated hover throttle, 0 ~ 1
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN 0.3f // minimum possible hover throttle
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX 0.8f // maximum possible hover throttle
// flybar types
#define AP_MOTORS_HELI_NOFLYBAR 0
@ -119,7 +123,11 @@ public:
// supports_yaw_passthrough
virtual bool supports_yaw_passthrough() const { return false; }
float get_throttle_hover() const override { return 0.5f; }
// update estimated throttle required to hover
void update_throttle_hover(float dt);
float get_throttle_hover() const override { return _collective_hover; }
bool get_takeoff_collective() const { return _heliflags.takeoff_collective; }
// support passing init_targets_on_arming flag to greater code
bool init_targets_on_arming() const override { return _heliflags.init_targets_on_arming; }
@ -132,6 +140,9 @@ public:
// return true if the servo test is still running/pending
bool servo_test_running() const { return _heliflags.servo_test_running; }
// set land complete flag
void set_land_complete(bool landed) { _heliflags.land_complete = landed; }
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -193,6 +204,16 @@ protected:
// write to a swash servo. output value is pwm
void rc_write_swash(uint8_t chan, float swash_in);
// save parameters as part of disarming
void save_params_on_disarm() override;
// enum values for HOVER_LEARN parameter
enum HoverLearn {
HOVER_LEARN_DISABLED = 0,
HOVER_LEARN_ONLY = 1,
HOVER_LEARN_AND_SAVE = 2
};
// flags bitmask
struct heliflags_type {
uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum
@ -203,6 +224,8 @@ protected:
uint8_t in_autorotation : 1; // true if aircraft is in autorotation
uint8_t enable_bailout : 1; // true if allowing RSC to quickly ramp up engine
uint8_t servo_test_running : 1; // true if servo_test is running
uint8_t land_complete : 1; // true if aircraft is landed
uint8_t takeoff_collective : 1; // true if collective is above 30% between H_COL_MID and H_COL_MAX
} _heliflags;
// parameters
@ -212,6 +235,8 @@ protected:
AP_Int16 _collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Asymmetrical blades)
AP_Int8 _servo_mode; // Pass radio inputs directly to servos during set-up through mission planner
AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup
AP_Float _collective_hover; // estimated collective required to hover throttle in the range 0 ~ 1
AP_Int8 _collective_hover_learn; // enable/disabled hover collective learning
// internal variables
float _collective_mid_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range

9
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -527,9 +527,6 @@ void AP_MotorsHeli_Dual::update_motor_control(RotorControlState state)
void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out)
{
// initialize limits flag
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
@ -576,6 +573,12 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
limit.throttle_lower = true;
}
if (collective_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) {
_heliflags.takeoff_collective = true;
} else {
_heliflags.takeoff_collective = false;
}
// Set rear collective to midpoint if required
float collective2_out = collective_out;
if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) {

9
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -223,9 +223,6 @@ void AP_MotorsHeli_Quad::update_motor_control(RotorControlState state)
void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out)
{
// initialize limits flag
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
@ -246,6 +243,12 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
limit.throttle_lower = true;
}
if (collective_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) {
_heliflags.takeoff_collective = true;
} else {
_heliflags.takeoff_collective = false;
}
float collective_range = (_collective_max - _collective_min) * 0.001f;
if (_heliflags.inverted_flight) {

9
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -408,9 +408,6 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
float yaw_offset = 0.0f;
// initialize limits flag
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
@ -449,6 +446,12 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
limit.throttle_lower = true;
}
if (collective_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) {
_heliflags.takeoff_collective = true;
} else {
_heliflags.takeoff_collective = false;
}
// if servo output not in manual mode and heli is not in autorotation, process pre-compensation factors
if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED && !_heliflags.in_autorotation) {
// rudder feed forward based on collective