Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_RPM.h"
17 :
18 : #if AP_RPM_ENABLED
19 :
20 : #include "RPM_Backend.h"
21 : #include "RPM_Pin.h"
22 : #include "RPM_SITL.h"
23 : #include "RPM_EFI.h"
24 : #include "RPM_Generator.h"
25 : #include "RPM_HarmonicNotch.h"
26 : #include "RPM_ESC_Telem.h"
27 : #include "RPM_DroneCAN.h"
28 :
29 : #include <AP_Logger/AP_Logger.h>
30 :
31 : extern const AP_HAL::HAL& hal;
32 :
33 : // table of user settable parameters
34 : const AP_Param::GroupInfo AP_RPM::var_info[] = {
35 : // 0-13 used by old param indexes before being moved to AP_RPM_Params
36 :
37 : // @Group: 1_
38 : // @Path: AP_RPM_Params.cpp
39 : AP_SUBGROUPINFO(_params[0], "1_", 14, AP_RPM, AP_RPM_Params),
40 :
41 : #if RPM_MAX_INSTANCES > 1
42 : // @Group: 2_
43 : // @Path: AP_RPM_Params.cpp
44 : AP_SUBGROUPINFO(_params[1], "2_", 15, AP_RPM, AP_RPM_Params),
45 : #endif
46 :
47 : #if RPM_MAX_INSTANCES > 2
48 : // @Group: 3_
49 : // @Path: AP_RPM_Params.cpp
50 : AP_SUBGROUPINFO(_params[2], "3_", 16, AP_RPM, AP_RPM_Params),
51 : #endif
52 :
53 : #if RPM_MAX_INSTANCES > 3
54 : // @Group: 4_
55 : // @Path: AP_RPM_Params.cpp
56 : AP_SUBGROUPINFO(_params[3], "4_", 17, AP_RPM, AP_RPM_Params),
57 : #endif
58 :
59 : AP_GROUPEND
60 : };
61 :
62 6294 : AP_RPM::AP_RPM(void)
63 : {
64 2098 : AP_Param::setup_object_defaults(this, var_info);
65 :
66 2098 : if (_singleton != nullptr) {
67 0 : AP_HAL::panic("AP_RPM must be singleton");
68 : }
69 2098 : _singleton = this;
70 2098 : }
71 :
72 : /*
73 : initialise the AP_RPM class.
74 : */
75 2086 : void AP_RPM::init(void)
76 : {
77 2086 : if (num_instances != 0) {
78 : // init called a 2nd time?
79 0 : return;
80 : }
81 :
82 6258 : for (uint8_t i=0; i<RPM_MAX_INSTANCES; i++) {
83 4172 : switch (_params[i].type) {
84 : #if AP_RPM_PIN_ENABLED
85 0 : case RPM_TYPE_PWM:
86 : case RPM_TYPE_PIN:
87 : // PWM option same as PIN option, for upgrade
88 0 : drivers[i] = NEW_NOTHROW AP_RPM_Pin(*this, i, state[i]);
89 0 : break;
90 : #endif // AP_RPM_PIN_ENABLED
91 : #if AP_RPM_ESC_TELEM_ENABLED
92 0 : case RPM_TYPE_ESC_TELEM:
93 0 : drivers[i] = NEW_NOTHROW AP_RPM_ESC_Telem(*this, i, state[i]);
94 0 : break;
95 : #endif // AP_RPM_ESC_TELEM_ENABLED
96 : #if AP_RPM_EFI_ENABLED
97 4 : case RPM_TYPE_EFI:
98 4 : drivers[i] = NEW_NOTHROW AP_RPM_EFI(*this, i, state[i]);
99 4 : break;
100 : #endif // AP_RPM_EFI_ENABLED
101 : #if AP_RPM_GENERATOR_ENABLED
102 0 : case RPM_TYPE_GENERATOR:
103 0 : drivers[i] = NEW_NOTHROW AP_RPM_Generator(*this, i, state[i]);
104 0 : break;
105 : #endif // AP_RPM_GENERATOR_ENABLED
106 : #if AP_RPM_HARMONICNOTCH_ENABLED
107 : // include harmonic notch last
108 : // this makes whatever process is driving the dynamic notch appear as an RPM value
109 0 : case RPM_TYPE_HNTCH:
110 0 : drivers[i] = NEW_NOTHROW AP_RPM_HarmonicNotch(*this, i, state[i]);
111 0 : break;
112 : #endif // AP_RPM_HARMONICNOTCH_ENABLED
113 : #if AP_RPM_DRONECAN_ENABLED
114 0 : case RPM_TYPE_DRONECAN:
115 0 : drivers[i] = NEW_NOTHROW AP_RPM_DroneCAN(*this, i, state[i]);
116 0 : break;
117 : #endif // AP_RPM_DRONECAN_ENABLED
118 : #if AP_RPM_SIM_ENABLED
119 93 : case RPM_TYPE_SITL:
120 93 : drivers[i] = NEW_NOTHROW AP_RPM_SITL(*this, i, state[i]);
121 93 : break;
122 : #endif // AP_RPM_SIM_ENABLED
123 : }
124 4172 : if (drivers[i] != nullptr) {
125 : // we loaded a driver for this instance, so it must be
126 : // present (although it may not be healthy)
127 97 : num_instances = i+1; // num_instances is a high-water-mark
128 : }
129 : }
130 : }
131 :
132 : /*
133 : update RPM state for all instances. This should be called by main loop
134 : */
135 7525129 : void AP_RPM::update(void)
136 : {
137 8010052 : for (uint8_t i=0; i<num_instances; i++) {
138 484923 : if (drivers[i] != nullptr) {
139 484923 : if (_params[i].type == RPM_TYPE_NONE) {
140 : // allow user to disable an RPM sensor at runtime and force it to re-learn the quality if re-enabled.
141 333 : state[i].signal_quality = 0;
142 333 : continue;
143 : }
144 :
145 484590 : drivers[i]->update();
146 :
147 : #if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED
148 484590 : drivers[i]->update_esc_telem_outbound();
149 : #endif
150 : }
151 : }
152 :
153 : #if HAL_LOGGING_ENABLED
154 7525129 : Log_RPM();
155 : #endif
156 7525129 : }
157 :
158 : /*
159 : check if an instance is healthy
160 : */
161 4372847 : bool AP_RPM::healthy(uint8_t instance) const
162 : {
163 4372847 : if (instance >= num_instances || _params[instance].type == RPM_TYPE_NONE) {
164 177529 : return false;
165 : }
166 :
167 : // check that data quality is above minimum required
168 4195318 : if (state[instance].signal_quality < _params[instance].quality_min) {
169 726 : return false;
170 : }
171 :
172 4194592 : return true;
173 : }
174 :
175 : /*
176 : check if an instance is activated
177 : */
178 2670788 : bool AP_RPM::enabled(uint8_t instance) const
179 : {
180 2670788 : if (instance >= num_instances) {
181 2129548 : return false;
182 : }
183 : // if no sensor type is selected, the sensor is not activated.
184 541240 : if (_params[instance].type == RPM_TYPE_NONE) {
185 389 : return false;
186 : }
187 540851 : return true;
188 : }
189 :
190 : /*
191 : get RPM value, return true on success
192 : */
193 3589740 : bool AP_RPM::get_rpm(uint8_t instance, float &rpm_value) const
194 : {
195 3589740 : if (!healthy(instance)) {
196 145544 : return false;
197 : }
198 3444196 : rpm_value = state[instance].rate_rpm;
199 3444196 : return true;
200 : }
201 :
202 : // check settings are valid
203 66519 : bool AP_RPM::arming_checks(size_t buflen, char *buffer) const
204 : {
205 199557 : for (uint8_t i=0; i<RPM_MAX_INSTANCES; i++) {
206 133038 : switch (_params[i].type) {
207 : #if AP_RPM_PIN_ENABLED
208 0 : case RPM_TYPE_PWM:
209 : case RPM_TYPE_PIN:
210 0 : if (_params[i].pin == -1) {
211 0 : hal.util->snprintf(buffer, buflen, "RPM%u_PIN not set", unsigned(i + 1));
212 0 : return false;
213 : }
214 0 : if (!hal.gpio->valid_pin(_params[i].pin)) {
215 : uint8_t servo_ch;
216 0 : if (hal.gpio->pin_to_servo_channel(_params[i].pin, servo_ch)) {
217 0 : hal.util->snprintf(buffer, buflen, "RPM%u_PIN=%d, set SERVO%u_FUNCTION=-1", unsigned(i + 1), int(_params[i].pin.get()), unsigned(servo_ch+1));
218 : } else {
219 0 : hal.util->snprintf(buffer, buflen, "RPM%u_PIN=%d invalid", unsigned(i + 1), int(_params[i].pin.get()));
220 : }
221 0 : return false;
222 : }
223 0 : break;
224 : #endif
225 : }
226 : }
227 66519 : return true;
228 : }
229 :
230 : #if HAL_LOGGING_ENABLED
231 7525129 : void AP_RPM::Log_RPM() const
232 : {
233 : // update logging for each instance
234 8010052 : for (uint8_t i=0; i<num_instances; i++) {
235 484923 : if (drivers[i] == nullptr || !enabled(i)) {
236 : // don't log unused instances
237 333 : continue;
238 : }
239 :
240 484590 : const struct log_RPM pkt{
241 : LOG_PACKET_HEADER_INIT(LOG_RPM_MSG),
242 484590 : time_us : AP_HAL::micros64(),
243 : inst : i,
244 484590 : rpm : state[i].rate_rpm,
245 484590 : quality : get_signal_quality(i),
246 969180 : health : uint8_t(healthy(i))
247 484590 : };
248 484590 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
249 : }
250 7525129 : }
251 : #endif
252 :
253 : #if AP_RPM_STREAM_ENABLED
254 : // Return the sensor id to use for streaming over DroneCAN, negative number disables
255 : int8_t AP_RPM::get_dronecan_sensor_id(uint8_t instance) const
256 : {
257 : if (!enabled(instance)) {
258 : return -1;
259 : }
260 : return _params[instance].dronecan_sensor_id;
261 : }
262 : #endif
263 :
264 :
265 : // singleton instance
266 : AP_RPM *AP_RPM::_singleton;
267 :
268 : namespace AP {
269 :
270 1494243 : AP_RPM *rpm()
271 : {
272 1494243 : return AP_RPM::get_singleton();
273 : }
274 :
275 : }
276 :
277 : #endif // AP_RPM_ENABLED
|
