mirror of https://github.com/ArduPilot/ardupilot
Copter: move implementation of get_pilot_desired_yaw_rate into mode.cpp
This commit is contained in:
parent
8bf1ac0347
commit
dc793e916f
|
@ -1,35 +1,5 @@
|
||||||
#include "Copter.h"
|
#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
|
* throttle control
|
||||||
****************************************************************/
|
****************************************************************/
|
||||||
|
|
|
@ -666,7 +666,6 @@ private:
|
||||||
void update_altitude();
|
void update_altitude();
|
||||||
|
|
||||||
// Attitude.cpp
|
// Attitude.cpp
|
||||||
float get_pilot_desired_yaw_rate(int16_t stick_angle);
|
|
||||||
void update_throttle_hover();
|
void update_throttle_hover();
|
||||||
float get_pilot_desired_climb_rate(float throttle_control);
|
float get_pilot_desired_climb_rate(float throttle_control);
|
||||||
float get_non_takeoff_throttle();
|
float get_non_takeoff_throttle();
|
||||||
|
|
|
@ -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;
|
// pass-through functions to reduce code churn on conversion;
|
||||||
// these are candidates for moving into the Mode base
|
// these are candidates for moving into the Mode base
|
||||||
// class.
|
// 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)
|
float Mode::get_pilot_desired_climb_rate(float throttle_control)
|
||||||
{
|
{
|
||||||
return copter.get_pilot_desired_climb_rate(throttle_control);
|
return copter.get_pilot_desired_climb_rate(throttle_control);
|
||||||
|
|
|
@ -250,7 +250,7 @@ void ModeFlowHold::run()
|
||||||
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), copter.g.pilot_speed_up);
|
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), copter.g.pilot_speed_up);
|
||||||
|
|
||||||
// get pilot's desired yaw rate
|
// 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
|
// Flow Hold State Machine Determination
|
||||||
AltHoldModeState flowhold_state = get_alt_hold_state(target_climb_rate);
|
AltHoldModeState flowhold_state = get_alt_hold_state(target_climb_rate);
|
||||||
|
|
Loading…
Reference in New Issue