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

Copter: move implementation of get_pilot_desired_yaw_rate into mode.cpp

This commit is contained in:
Peter Barker 2021-01-27 10:06:20 +11:00 committed by Peter Barker
parent 8bf1ac0347
commit dc793e916f
4 changed files with 31 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduCopter/Attitude.cpp
View File

@ -1,35 +1,5 @@
#include "Copter.h"
// transform pilot's yaw input into a desired yaw rate
// returns desired yaw rate in centi-degrees per second
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
// throttle failsafe check
if (failsafe.radio || !ap.rc_receiver_present) {
return 0.0f;
}
float yaw_request;
// range check expo
g2.acro_y_expo = constrain_float(g2.acro_y_expo, 0.0f, 1.0f);
// calculate yaw rate request
if (is_zero(g2.acro_y_expo)) {
yaw_request = stick_angle * g.acro_yaw_p;
} else {
// expo variables
float y_in, y_in3, y_out;
// yaw expo
y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;
y_in3 = y_in*y_in*y_in;
y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in);
yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * g.acro_yaw_p;
}
// convert pilot input to the desired yaw rate
return yaw_request;
}
/*************************************************************
* throttle control
****************************************************************/

1
ArduCopter/Copter.h
View File

@ -666,7 +666,6 @@ private:
void update_altitude();
// Attitude.cpp
float get_pilot_desired_yaw_rate(int16_t stick_angle);
void update_throttle_hover();
float get_pilot_desired_climb_rate(float throttle_control);
float get_non_takeoff_throttle();

35
ArduCopter/mode.cpp
View File

@ -773,14 +773,39 @@ Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms)
}
}
// transform pilot's yaw input into a desired yaw rate
// returns desired yaw rate in centi-degrees per second
float Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
// throttle failsafe check
if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
return 0.0f;
}
float yaw_request;
// range check expo
g2.acro_y_expo = constrain_float(g2.acro_y_expo, 0.0f, 1.0f);
// calculate yaw rate request
if (is_zero(g2.acro_y_expo)) {
yaw_request = stick_angle * g.acro_yaw_p;
} else {
// expo variables
float y_in, y_in3, y_out;
// yaw expo
y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;
y_in3 = y_in*y_in*y_in;
y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in);
yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * g.acro_yaw_p;
}
// convert pilot input to the desired yaw rate
return yaw_request;
}
// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
float Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
return copter.get_pilot_desired_yaw_rate(stick_angle);
}
float Mode::get_pilot_desired_climb_rate(float throttle_control)
{
return copter.get_pilot_desired_climb_rate(throttle_control);

2
ArduCopter/mode_flowhold.cpp
View File

@ -250,7 +250,7 @@ void ModeFlowHold::run()
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), copter.g.pilot_speed_up);
// get pilot's desired yaw rate
float target_yaw_rate = copter.get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in());
float target_yaw_rate = get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in());
// Flow Hold State Machine Determination
AltHoldModeState flowhold_state = get_alt_hold_state(target_climb_rate);