Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #include "AC_CustomControl.h"
4 :
5 : #if AP_COPTER_CUSTOMCONTROL_ENABLED
6 :
7 : #include "AC_CustomControl_Backend.h"
8 : // #include "AC_CustomControl_Empty.h"
9 : #include "AC_CustomControl_PID.h"
10 : #include <GCS_MAVLink/GCS.h>
11 : #include <AP_Logger/AP_Logger.h>
12 : #include <AP_Scheduler/AP_Scheduler.h>
13 :
14 : // table of user settable parameters
15 : const AP_Param::GroupInfo AC_CustomControl::var_info[] = {
16 : // @Param: _TYPE
17 : // @DisplayName: Custom control type
18 : // @Description: Custom control type to be used
19 : // @Values: 0:None, 1:Empty, 2:PID
20 : // @RebootRequired: True
21 : // @User: Advanced
22 : AP_GROUPINFO_FLAGS("_TYPE", 1, AC_CustomControl, _controller_type, 0, AP_PARAM_FLAG_ENABLE),
23 :
24 : // @Param: _AXIS_MASK
25 : // @DisplayName: Custom Controller bitmask
26 : // @Description: Custom Controller bitmask to chose which axis to run
27 : // @Bitmask: 0:Roll, 1:Pitch, 2:Yaw
28 : // @User: Advanced
29 : AP_GROUPINFO("_AXIS_MASK", 2, AC_CustomControl, _custom_controller_mask, 0),
30 :
31 : // parameters for empty controller. only used as a template, no need for param table
32 : // AP_SUBGROUPVARPTR(_backend, "1_", 6, AC_CustomControl, _backend_var_info[0]),
33 :
34 : // @Group: 2_
35 : // @Path: AC_CustomControl_PID.cpp
36 : AP_SUBGROUPVARPTR(_backend, "2_", 7, AC_CustomControl, _backend_var_info[1]),
37 :
38 : AP_GROUPEND
39 : };
40 :
41 : const struct AP_Param::GroupInfo *AC_CustomControl::_backend_var_info[CUSTOMCONTROL_MAX_TYPES];
42 :
43 918 : AC_CustomControl::AC_CustomControl(AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors) :
44 918 : _ahrs(ahrs),
45 918 : _att_control(att_control),
46 918 : _motors(motors)
47 : {
48 918 : AP_Param::setup_object_defaults(this, var_info);
49 918 : }
50 :
51 916 : void AC_CustomControl::init(void)
52 : {
53 916 : _dt = AP::scheduler().get_loop_period_s();
54 :
55 916 : switch (CustomControlType(_controller_type))
56 : {
57 914 : case CustomControlType::CONT_NONE:
58 914 : break;
59 0 : case CustomControlType::CONT_EMPTY: // This is template backend. Don't initialize it.
60 : // This is template backend. Don't initialize it.
61 : // _backend = NEW_NOTHROW AC_CustomControl_Empty(*this, _ahrs, _att_control, _motors, _dt);
62 : // _backend_var_info[get_type()] = AC_CustomControl_Empty::var_info;
63 0 : break;
64 2 : case CustomControlType::CONT_PID:
65 2 : _backend = NEW_NOTHROW AC_CustomControl_PID(*this, _ahrs, _att_control, _motors, _dt);
66 2 : _backend_var_info[get_type()] = AC_CustomControl_PID::var_info;
67 2 : break;
68 0 : default:
69 0 : return;
70 : }
71 :
72 916 : if (_backend && _backend_var_info[get_type()]) {
73 2 : AP_Param::load_object_from_eeprom(_backend, _backend_var_info[get_type()]);
74 : }
75 : }
76 :
77 : // run custom controller if it is activated by RC switch and appropriate type is selected
78 27178469 : void AC_CustomControl::update(void)
79 : {
80 27178469 : if (is_safe_to_run()) {
81 8400 : Vector3f motor_out_rpy;
82 :
83 8400 : motor_out_rpy = _backend->update();
84 :
85 8400 : motor_set(motor_out_rpy);
86 : }
87 27178469 : }
88 :
89 : // choose which axis to apply custom controller output
90 8400 : void AC_CustomControl::motor_set(Vector3f rpy) {
91 8400 : if (_custom_controller_mask & (uint8_t)CustomControlOption::ROLL) {
92 8400 : _motors->set_roll(rpy.x);
93 8400 : _att_control->get_rate_roll_pid().set_integrator(0.0);
94 : }
95 8400 : if (_custom_controller_mask & (uint8_t)CustomControlOption::PITCH) {
96 8400 : _motors->set_pitch(rpy.y);
97 8400 : _att_control->get_rate_pitch_pid().set_integrator(0.0);
98 : }
99 8400 : if (_custom_controller_mask & (uint8_t)CustomControlOption::YAW) {
100 8400 : _motors->set_yaw(rpy.z);
101 8400 : _att_control->get_rate_yaw_pid().set_integrator(0.0);
102 : }
103 8400 : }
104 :
105 : // move main controller's target to current states, reset filters,
106 : // and move integrator to motor output
107 : // to allow smooth transition to the primary controller
108 2 : void AC_CustomControl::reset_main_att_controller(void)
109 : {
110 : // reset attitude and rate target, if feedforward is enabled
111 2 : if (_att_control->get_bf_feedforward()) {
112 2 : _att_control->relax_attitude_controllers();
113 : }
114 :
115 2 : _att_control->get_rate_roll_pid().set_integrator(0.0);
116 2 : _att_control->get_rate_pitch_pid().set_integrator(0.0);
117 2 : _att_control->get_rate_yaw_pid().set_integrator(0.0);
118 2 : }
119 :
120 6 : void AC_CustomControl::set_custom_controller(bool enabled)
121 : {
122 : // double logging switch makes the state change very clear in the log
123 6 : log_switch();
124 :
125 6 : _custom_controller_active = false;
126 :
127 : // don't allow accidental main controller reset without active custom controller
128 6 : if (_controller_type == CustomControlType::CONT_NONE) {
129 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Custom controller is not enabled");
130 0 : return;
131 : }
132 :
133 : // controller type is out of range
134 6 : if (_controller_type > CUSTOMCONTROL_MAX_TYPES) {
135 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Custom controller type is out of range");
136 0 : return;
137 : }
138 :
139 : // backend is not created
140 6 : if (_backend == nullptr) {
141 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Reboot to enable selected custom controller");
142 3 : return;
143 : }
144 :
145 3 : if (_custom_controller_mask == 0 && enabled) {
146 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Axis mask is not set");
147 0 : return;
148 : }
149 :
150 : // reset main controller
151 3 : if (!enabled) {
152 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Custom controller is OFF");
153 : // don't reset if the empty backend is selected
154 2 : if (_controller_type > CustomControlType::CONT_EMPTY) {
155 2 : reset_main_att_controller();
156 : }
157 : }
158 :
159 3 : if (enabled && _controller_type > CustomControlType::CONT_NONE) {
160 : // reset custom controller filter, integrator etc.
161 1 : _backend->reset();
162 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Custom controller is ON");
163 : }
164 :
165 3 : _custom_controller_active = enabled;
166 :
167 : // log successful switch
168 3 : log_switch();
169 : }
170 :
171 : // check that RC switch is on, backend is not changed mid flight and controller type is selected
172 27178469 : bool AC_CustomControl::is_safe_to_run(void) {
173 8400 : if (_custom_controller_active && (_controller_type > CustomControlType::CONT_NONE)
174 27186869 : && (_controller_type <= CUSTOMCONTROL_MAX_TYPES) && _backend != nullptr)
175 : {
176 8400 : return true;
177 : }
178 :
179 27170069 : return false;
180 : }
181 :
182 : // log when the custom controller is switch into
183 9 : void AC_CustomControl::log_switch(void) {
184 : // @LoggerMessage: CC
185 : // @Description: Custom Controller data
186 : // @Field: TimeUS: Time since system startup
187 : // @Field: Type: controller type
188 : // @FieldValueEnum: Type: AC_CustomControl::CustomControlType
189 : // @Field: Act: true if controller is active
190 18 : AP::logger().Write("CC", "TimeUS,Type,Act","QBB",
191 : AP_HAL::micros64(),
192 : _controller_type,
193 9 : _custom_controller_active);
194 9 : }
195 :
196 67166 : void AC_CustomControl::set_notch_sample_rate(float sample_rate)
197 : {
198 : #if AP_FILTER_ENABLED
199 67166 : if (_backend != nullptr) {
200 117 : _backend->set_notch_sample_rate(sample_rate);
201 : }
202 : #endif
203 67166 : }
204 :
205 : #endif // AP_COPTER_CUSTOMCONTROL_ENABLED
|
