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

Plane: added manual input expo for MANUAL, ACRO and TRAINING 

this is easier than setting up mode specific expo in radios for flying
in manual modes
This commit is contained in:
Andrew Tridgell 2021-07-17 16:40:38 +10:00 committed by Randy Mackay
parent 0b76a17e65
commit d67f82e02d
8 changed files with 84 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduPlane/Attitude.cpp
View File

@ -312,26 +312,28 @@ void Plane::stabilize_yaw(float speed_scaler)
*/
void Plane::stabilize_training(float speed_scaler)
{
const float rexpo = roll_in_expo(false);
const float pexpo = pitch_in_expo(false);
if (training_manual_roll) {
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
} else {
// calculate what is needed to hold
stabilize_roll(speed_scaler);
if ((nav_roll_cd > 0 && channel_roll->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
(nav_roll_cd < 0 && channel_roll->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
if ((nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
(nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
// allow user to get out of the roll
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
}
}
if (training_manual_pitch) {
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
} else {
stabilize_pitch(speed_scaler);
if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
(nav_pitch_cd < 0 && channel_pitch->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
if ((nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
(nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
// allow user to get back to level
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
}
}
@ -345,8 +347,10 @@ void Plane::stabilize_training(float speed_scaler)
*/
void Plane::stabilize_acro(float speed_scaler)
{
float roll_rate = (channel_roll->get_control_in()/4500.0f) * g.acro_roll_rate;
float pitch_rate = (channel_pitch->get_control_in()/4500.0f) * g.acro_pitch_rate;
const float rexpo = roll_in_expo(true);
const float pexpo = pitch_in_expo(true);
float roll_rate = (rexpo/float(SERVO_MAX)) * g.acro_roll_rate;
float pitch_rate = (pexpo/float(SERVO_MAX)) * g.acro_pitch_rate;
/*
check for special roll handling near the pitch poles

24
ArduPlane/Parameters.cpp
View File

@ -1225,6 +1225,30 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(guidedHeading, "GUIDED_", 28, ParametersG2, AC_PID),
#endif // OFFBOARD_GUIDED == ENABLED
// @Param: MAN_EXPO_ROLL
// @DisplayName: Manual control expo for roll
// @Description: Percentage exponential for roll input in MANUAL, ACRO and TRAINING modes
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("MAN_EXPO_ROLL", 29, ParametersG2, man_expo_roll, 0),
// @Param: MAN_EXPO_PITCH
// @DisplayName: Manual input expo for pitch
// @Description: Percentage exponential for pitch input in MANUAL, ACRO and TRAINING modes
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("MAN_EXPO_PITCH", 30, ParametersG2, man_expo_pitch, 0),
// @Param: MAN_EXPO_RUDDER
// @DisplayName: Manual input expo for rudder
// @Description: Percentage exponential for rudder input in MANUAL, ACRO and TRAINING modes
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("MAN_EXPO_RUDDER", 31, ParametersG2, man_expo_rudder, 0),
AP_GROUPEND
};

4
ArduPlane/Parameters.h
View File

@ -569,6 +569,10 @@ public:
// min initial climb in RTL
AP_Int16 rtl_climb_min;
AP_Int8 man_expo_roll;
AP_Int8 man_expo_pitch;
AP_Int8 man_expo_rudder;
};
extern const AP_Param::Info var_info[];

5
ArduPlane/Plane.h
View File

@ -1139,6 +1139,11 @@ private:
FlareMode flare_mode;
// expo handling
float roll_in_expo(bool use_dz) const;
float pitch_in_expo(bool use_dz) const;
float rudder_in_expo(bool use_dz) const;
public:
void failsafe_check(void);
#ifdef ENABLE_SCRIPTING

6
ArduPlane/failsafe.cpp
View File

@ -63,10 +63,10 @@ void Plane::failsafe_check(void)
// pass RC inputs to outputs every 20ms
RC_Channels::clear_overrides();
int16_t roll = channel_roll->get_control_in_zero_dz();
int16_t pitch = channel_pitch->get_control_in_zero_dz();
int16_t roll = roll_in_expo(false);
int16_t pitch = pitch_in_expo(false);
int16_t throttle = get_throttle_input(true);
int16_t rudder = channel_rudder->get_control_in_zero_dz();
int16_t rudder = rudder_in_expo(false);
if (!hal.util->get_soft_armed()) {
throttle = 0;

6
ArduPlane/mode_manual.cpp
View File

@ -10,8 +10,8 @@ void ModeManual::_exit()
void ModeManual::update()
{
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz());
plane.steering_control.steering = plane.steering_control.rudder = plane.channel_rudder->get_control_in_zero_dz();
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.roll_in_expo(false));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false));
plane.steering_control.steering = plane.steering_control.rudder = plane.rudder_in_expo(false);
}

27
ArduPlane/radio.cpp
View File

@ -390,3 +390,30 @@ bool Plane::rc_failsafe_active(void) const
}
return false;
}
/*
expo handling for MANUAL, ACRO and TRAINING modes
*/
static float channel_expo(RC_Channel *chan, int8_t expo, bool use_dz)
{
if (chan == nullptr) {
return 0;
}
float rin = use_dz? chan->get_control_in() : chan->get_control_in_zero_dz();
return SERVO_MAX * expo_curve(constrain_float(expo*0.01, 0, 1), rin/SERVO_MAX);
}
float Plane::roll_in_expo(bool use_dz) const
{
return channel_expo(channel_roll, g2.man_expo_roll, use_dz);
}
float Plane::pitch_in_expo(bool use_dz) const
{
return channel_expo(channel_pitch, g2.man_expo_roll, use_dz);
}
float Plane::rudder_in_expo(bool use_dz) const
{
return channel_expo(channel_rudder, g2.man_expo_roll, use_dz);
}

8
ArduPlane/servos.cpp
View File

@ -362,9 +362,9 @@ void Plane::set_servos_idle(void)
*/
void Plane::set_servos_manual_passthrough(void)
{
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, channel_rudder->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_in_expo(false));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_in_expo(false));
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_in_expo(false));
int8_t throttle = get_throttle_input(true);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
@ -818,7 +818,7 @@ void Plane::set_servos(void)
steering_control.ground_steering = false;
if (control_mode == &mode_training) {
steering_control.rudder = channel_rudder->get_control_in();
steering_control.rudder = rudder_in_expo(false);
}
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);