forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 6
/
mode_qloiter.cpp
133 lines (108 loc) · 4.84 KB
/
mode_qloiter.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
#include "mode.h"
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
bool ModeQLoiter::_enter()
{
// initialise loiter
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100);
pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100);
quadplane.init_throttle_wait();
// prevent re-init of target position
quadplane.last_loiter_ms = AP_HAL::millis();
return true;
}
void ModeQLoiter::update()
{
plane.mode_qstabilize.update();
}
// run quadplane loiter controller
void ModeQLoiter::run()
{
const uint32_t now = AP_HAL::millis();
if (quadplane.tailsitter.in_vtol_transition(now)) {
// Tailsitters in FW pull up phase of VTOL transition run FW controllers
Mode::run();
return;
}
if (quadplane.throttle_wait) {
quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control->set_throttle_out(0, true, 0);
quadplane.relax_attitude_control();
pos_control->relax_z_controller(0);
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// Stabilize with fixed wing surfaces
plane.stabilize_roll();
plane.stabilize_pitch();
return;
}
if (!quadplane.motors->armed()) {
plane.mode_qloiter._enter();
}
if (quadplane.should_relax()) {
loiter_nav->soften_for_landing();
}
if (now - quadplane.last_loiter_ms > 500) {
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
}
quadplane.last_loiter_ms = now;
// motors use full range
quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100);
// process pilot's roll and pitch input
float target_roll_cd, target_pitch_cd;
quadplane.get_pilot_desired_lean_angles(target_roll_cd, target_pitch_cd, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
loiter_nav->set_pilot_desired_acceleration(target_roll_cd, target_pitch_cd);
// run loiter controller
if (!pos_control->is_active_xy()) {
pos_control->init_xy_controller();
}
loiter_nav->update();
// nav roll and pitch are controller by loiter controller
plane.nav_roll_cd = loiter_nav->get_roll();
plane.nav_pitch_cd = loiter_nav->get_pitch();
plane.quadplane.assign_tilt_to_fwd_thr();
if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
pos_control->set_externally_limited_xy();
}
// Pilot input, use yaw rate time constant
quadplane.set_pilot_yaw_rate_time_constant();
// call attitude controller with conservative smoothing gain of 4.0f
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
quadplane.get_desired_yaw_rate_cds());
if (plane.control_mode == &plane.mode_qland) {
if (poscontrol.get_state() < QuadPlane::QPOS_LAND_FINAL && quadplane.check_land_final()) {
poscontrol.set_state(QuadPlane::QPOS_LAND_FINAL);
quadplane.setup_target_position();
#if AP_ICENGINE_ENABLED
// cut IC engine if enabled
if (quadplane.land_icengine_cut != 0) {
plane.g2.ice_control.engine_control(0, 0, 0, false);
}
#endif // AP_ICENGINE_ENABLED
}
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
float descent_rate_cms = quadplane.landing_descent_rate_cms(height_above_ground);
if (poscontrol.get_state() == QuadPlane::QPOS_LAND_FINAL && !quadplane.option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) {
ahrs.set_touchdown_expected(true);
}
pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0);
quadplane.check_land_complete();
} else if (plane.control_mode == &plane.mode_guided && quadplane.guided_takeoff) {
quadplane.set_climb_rate_cms(0);
} else {
// update altitude target and call position controller
quadplane.set_climb_rate_cms(quadplane.get_pilot_desired_climb_rate_cms());
}
quadplane.run_z_controller();
// Stabilize with fixed wing surfaces
plane.stabilize_roll();
plane.stabilize_pitch();
}
#endif