-
Notifications
You must be signed in to change notification settings - Fork 0
/
Attitude.cpp
147 lines (126 loc) · 4.83 KB
/
Attitude.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
#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
****************************************************************/
// update estimated throttle required to hover (if necessary)
// called at 100hz
void Copter::update_throttle_hover()
{
#if FRAME_CONFIG != HELI_FRAME
// if not armed or landed exit
if (!motors->armed() || ap.land_complete) {
return;
}
// do not update in manual throttle modes or Drift
if (flightmode->has_manual_throttle() || (control_mode == Mode::Number::DRIFT)) {
return;
}
// do not update while climbing or descending
if (!is_zero(pos_control->get_desired_velocity().z)) {
return;
}
// get throttle output
float throttle = motors->get_throttle();
// calc average throttle if we are in a level hover
if (throttle > 0.0f && fabsf(inertial_nav.get_velocity_z()) < 60 &&
labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
// Can we set the time constant automatically
motors->update_throttle_hover(0.01f);
#if HAL_GYROFFT_ENABLED
gyro_fft.update_freq_hover(0.01f, motors->get_throttle_out());
#endif
}
#endif
}
// get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
// without any deadzone at the bottom
float Copter::get_pilot_desired_climb_rate(float throttle_control)
{
// throttle failsafe check
if (failsafe.radio || !ap.rc_receiver_present) {
return 0.0f;
}
#if TOY_MODE_ENABLED == ENABLED
if (g2.toy_mode.enabled()) {
// allow throttle to be reduced after throttle arming and for
// slower descent close to the ground
g2.toy_mode.throttle_adjust(throttle_control);
}
#endif
float desired_rate = 0.0f;
float mid_stick = get_throttle_mid();
float deadband_top = mid_stick + g.throttle_deadzone;
float deadband_bottom = mid_stick - g.throttle_deadzone;
// ensure a reasonable throttle value
throttle_control = constrain_float(throttle_control,0.0f,1000.0f);
// ensure a reasonable deadzone
g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);
// check throttle is above, below or in the deadband
if (throttle_control < deadband_bottom) {
// below the deadband
desired_rate = get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom;
} else if (throttle_control > deadband_top) {
// above the deadband
desired_rate = g.pilot_speed_up * (throttle_control-deadband_top) / (1000.0f-deadband_top);
} else {
// must be in the deadband
desired_rate = 0.0f;
}
return desired_rate;
}
// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
float Copter::get_non_takeoff_throttle()
{
return MAX(0,motors->get_throttle_hover()/2.0f);
}
// set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle
void Copter::set_accel_throttle_I_from_pilot_throttle()
{
// get last throttle input sent to attitude controller
float pilot_throttle = constrain_float(attitude_control->get_throttle_in(), 0.0f, 1.0f);
// shift difference between pilot's throttle and hover throttle into accelerometer I
pos_control->get_accel_z_pid().set_integrator((pilot_throttle-motors->get_throttle_hover()) * 1000.0f);
}
// rotate vector from vehicle's perspective to North-East frame
void Copter::rotate_body_frame_to_NE(float &x, float &y)
{
float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw();
float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
x = ne_x;
y = ne_y;
}
// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
uint16_t Copter::get_pilot_speed_dn()
{
if (g2.pilot_speed_dn == 0) {
return abs(g.pilot_speed_up);
} else {
return abs(g2.pilot_speed_dn);
}
}