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 : * AP_KDECAN.cpp
17 : *
18 : * Author: Francisco Ferreira and Tom Pittenger
19 : */
20 :
21 : #include "AP_KDECAN.h"
22 :
23 : #if AP_KDECAN_ENABLED
24 : #include <stdio.h>
25 : #include <AP_BoardConfig/AP_BoardConfig.h>
26 : #include <AP_HAL/utility/sparse-endian.h>
27 : #include <SRV_Channel/SRV_Channel.h>
28 : #include <GCS_MAVLink/GCS.h>
29 : #include <AP_Math/AP_Math.h> // for MIN,MAX
30 :
31 : extern const AP_HAL::HAL& hal;
32 :
33 : #define AP_KDECAN_DEBUG 0
34 :
35 : // table of user settable CAN bus parameters
36 : const AP_Param::GroupInfo AP_KDECAN::var_info[] = {
37 :
38 : // @Param: NPOLE
39 : // @DisplayName: Number of motor poles
40 : // @Description: Sets the number of motor poles to calculate the correct RPM value
41 : AP_GROUPINFO("NPOLE", 1, AP_KDECAN, _num_poles, DEFAULT_NUM_POLES),
42 :
43 : AP_GROUPEND
44 : };
45 :
46 2098 : AP_KDECAN::AP_KDECAN()
47 : {
48 2098 : AP_Param::setup_object_defaults(this, var_info);
49 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
50 2096 : if (_singleton != nullptr) {
51 0 : AP_HAL::panic("AP_KDECAN must be singleton");
52 : }
53 : #endif
54 2098 : _singleton = this;
55 2098 : }
56 :
57 2086 : void AP_KDECAN::init()
58 : {
59 2086 : if (_driver != nullptr) {
60 : // only allow one instance
61 0 : return;
62 : }
63 :
64 6258 : for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
65 4172 : if (CANSensor::get_driver_type(i) == AP_CAN::Protocol::KDECAN) {
66 0 : _driver = NEW_NOTHROW AP_KDECAN_Driver();
67 0 : return;
68 : }
69 : }
70 : }
71 :
72 41692164 : void AP_KDECAN::update()
73 : {
74 41692164 : if (_driver == nullptr) {
75 41692164 : return;
76 : }
77 0 : _driver->update((uint8_t)_num_poles.get());
78 : }
79 :
80 0 : AP_KDECAN_Driver::AP_KDECAN_Driver() : CANSensor("KDECAN")
81 : {
82 0 : register_driver(AP_CAN::Protocol::KDECAN);
83 :
84 : // start thread for receiving and sending CAN frames. Tests show we use about 640 bytes of stack
85 0 : hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_KDECAN_Driver::loop, void), "kdecan", 2048, AP_HAL::Scheduler::PRIORITY_CAN, 0);
86 0 : }
87 :
88 : // parse inbound frames
89 0 : void AP_KDECAN_Driver::handle_frame(AP_HAL::CANFrame &frame)
90 : {
91 0 : if (!frame.isExtended()) {
92 0 : return;
93 : }
94 :
95 0 : const frame_id_t id { .value = frame.id & AP_HAL::CANFrame::MaskExtID };
96 :
97 : #if AP_KDECAN_DEBUG
98 : if (id.object_address != TELEMETRY_OBJ_ADDR) {
99 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"KDECAN: rx id:%d, src:%d, dest:%d, len:%d", (int)id.object_address, (int)id.source_id, (int)id.destination_id, (int)frame.dlc);
100 : }
101 : #endif
102 :
103 0 : if (id.destination_id != AUTOPILOT_NODE_ID || id.source_id < ESC_NODE_ID_FIRST) {
104 : // not for us or invalid id (0 and 1 are invalid)
105 0 : return;
106 : }
107 :
108 : // check if frame is valid: directed at autopilot, doesn't come from broadcast and ESC was detected before
109 0 : switch (id.object_address) {
110 0 : case ESC_INFO_OBJ_ADDR:
111 0 : if (frame.dlc == 5 &&
112 0 : (id.source_id < (ARRAY_SIZE(_output.pwm) + ESC_NODE_ID_FIRST)))
113 : {
114 0 : if (__builtin_popcount(_init.detected_bitmask) >= KDECAN_MAX_NUM_ESCS) {
115 : // we already have the maximum number of ESCs
116 0 : return;
117 : }
118 0 : const uint16_t bitmask = (1UL << (id.source_id - ESC_NODE_ID_FIRST));
119 :
120 0 : if ((bitmask & _init.detected_bitmask) != bitmask) {
121 0 : _init.detected_bitmask |= bitmask;
122 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"KDECAN: Found ESC id %u mapped to SERVO%u", id.source_id, id.source_id-1);
123 : }
124 : }
125 0 : break;
126 :
127 : #if HAL_WITH_ESC_TELEM
128 0 : case TELEMETRY_OBJ_ADDR:
129 0 : if (frame.dlc == 8 &&
130 0 : (1UL << (id.source_id - ESC_NODE_ID_FIRST) & _init.detected_bitmask))
131 : {
132 0 : const uint8_t idx = id.source_id - ESC_NODE_ID_FIRST;
133 0 : const uint8_t num_poles = _telemetry.num_poles > 0 ? _telemetry.num_poles : DEFAULT_NUM_POLES;
134 0 : update_rpm(idx, uint16_t(uint16_t(frame.data[4] << 8 | frame.data[5]) * 60UL * 2 / num_poles));
135 :
136 0 : const TelemetryData t {
137 0 : .temperature_cdeg = int16_t(frame.data[6] * 100),
138 0 : .voltage = float(uint16_t(frame.data[0] << 8 | frame.data[1])) * 0.01f,
139 0 : .current = float(uint16_t(frame.data[2] << 8 | frame.data[3])) * 0.01f,
140 0 : };
141 0 : update_telem_data(idx, t,
142 : AP_ESC_Telem_Backend::TelemetryType::CURRENT |
143 : AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
144 : AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
145 : }
146 0 : break;
147 : #endif // HAL_WITH_ESC_TELEM
148 : }
149 : }
150 :
151 0 : void AP_KDECAN_Driver::update(const uint8_t num_poles)
152 : {
153 0 : if (_init.detected_bitmask == 0) {
154 : // nothing to do...
155 0 : return;
156 : }
157 :
158 : #if HAL_WITH_ESC_TELEM
159 0 : _telemetry.num_poles = num_poles;
160 : #endif
161 :
162 0 : WITH_SEMAPHORE(_output.sem);
163 0 : for (uint8_t i = 0; i < ARRAY_SIZE(_output.pwm); i++) {
164 0 : if ((_init.detected_bitmask & (1UL<<i)) == 0 || SRV_Channels::channel_function(i) <= SRV_Channel::Function::k_none) {
165 0 : _output.pwm[i] = 0;
166 0 : continue;
167 : }
168 :
169 0 : const SRV_Channel *c = SRV_Channels::srv_channel(i);
170 0 : if (c == nullptr) {
171 0 : _output.pwm[i] = 0;
172 0 : continue;
173 : }
174 0 : _output.pwm[i] = c->get_output_pwm();
175 : }
176 :
177 0 : _output.is_new = true;
178 :
179 : #if AP_KDECAN_USE_EVENTS
180 : if (_output.thread_ctx != nullptr) {
181 : // trigger the thread to wake up immediately
182 : chEvtSignal(_output.thread_ctx, 1);
183 : }
184 : #endif
185 :
186 : #if AP_KDECAN_DEBUG
187 : static uint32_t last_send_ms = 0;
188 : const uint32_t now_ms = AP_HAL::millis();
189 : if (now_ms - last_send_ms > 1000) {
190 : last_send_ms = now_ms;
191 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"%u: %u, %u, %u, %u, %u, %u, %u, %u",
192 : (unsigned)_init.detected_bitmask,
193 : (unsigned)_output.pwm[0], (unsigned)_output.pwm[1], (unsigned)_output.pwm[2], (unsigned)_output.pwm[3],
194 : (unsigned)_output.pwm[4], (unsigned)_output.pwm[5], (unsigned)_output.pwm[6], (unsigned)_output.pwm[7]);
195 : }
196 : #endif
197 0 : }
198 :
199 0 : void AP_KDECAN_Driver::loop()
200 : {
201 0 : uint16_t pwm[ARRAY_SIZE(_output.pwm)] {};
202 :
203 : #if AP_KDECAN_USE_EVENTS
204 : _output.thread_ctx = chThdGetSelfX();
205 : #endif
206 :
207 0 : uint8_t broadcast_esc_info_boot_spam_count = 3;
208 0 : uint32_t broadcast_esc_info_next_interval_ms = 100; // spam a few at boot at this rate
209 :
210 : while (true) {
211 : #if AP_KDECAN_USE_EVENTS
212 : // sleep until we get new data, but also wake up at 400Hz to send the old data again
213 : chEvtWaitAnyTimeout(ALL_EVENTS, chTimeUS2I(2500));
214 : #else
215 0 : hal.scheduler->delay_microseconds(2500); // 400Hz
216 : #endif
217 :
218 0 : const uint32_t now_ms = AP_HAL::millis();
219 :
220 : // This should run at 400Hz
221 : {
222 0 : WITH_SEMAPHORE(_output.sem);
223 0 : if (_output.is_new) {
224 0 : _output.last_new_ms = now_ms;
225 0 : _output.is_new = false;
226 0 : memcpy(&pwm, &_output.pwm, sizeof(pwm));
227 :
228 0 : } else if (_output.last_new_ms && now_ms - _output.last_new_ms > 1000) {
229 : // if we haven't gotten any PWM updates for a bit, zero it
230 : // out so we don't just keep sending the same values forever
231 0 : memset(&pwm, 0, sizeof(pwm));
232 0 : _output.last_new_ms = 0;
233 : }
234 0 : }
235 :
236 0 : for (uint8_t i=0; i<ARRAY_SIZE(_output.pwm); i++) {
237 0 : if ((_init.detected_bitmask & (1UL<<i)) != 0) {
238 0 : send_packet_uint16(SET_PWM_OBJ_ADDR, (i + ESC_NODE_ID_FIRST), 1000, pwm[i]);
239 : }
240 : }
241 :
242 : #if HAL_WITH_ESC_TELEM
243 : // broadcast as request-telemetry msg to everyone
244 0 : if (_init.detected_bitmask != 0 && now_ms - _telemetry.timer_ms >= TELEMETRY_INTERVAL_MS) {
245 0 : if (send_packet(TELEMETRY_OBJ_ADDR, BROADCAST_NODE_ID, 10000)) {
246 0 : _telemetry.timer_ms = now_ms;
247 : }
248 : }
249 : #endif // HAL_WITH_ESC_TELEM
250 :
251 0 : if ((_init.detected_bitmask == 0 || broadcast_esc_info_boot_spam_count > 0) && (now_ms - _init.detected_bitmask_ms >= broadcast_esc_info_next_interval_ms)) {
252 : // broadcast an "anyone there?" quick at boot but then 1Hz forever until we see at least 1 esc respond
253 0 : if (broadcast_esc_info_boot_spam_count > 0) {
254 0 : broadcast_esc_info_boot_spam_count--;
255 : } else {
256 0 : broadcast_esc_info_next_interval_ms = 1000;
257 : }
258 :
259 0 : if (send_packet(ESC_INFO_OBJ_ADDR, BROADCAST_NODE_ID, 100000)) {
260 0 : _init.detected_bitmask_ms = now_ms;
261 : }
262 : }
263 :
264 0 : } // while true
265 : }
266 :
267 0 : bool AP_KDECAN_Driver::send_packet_uint16(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_us, const uint16_t data)
268 : {
269 0 : const uint16_t data_be16 = htobe16(data);
270 0 : return send_packet(address, dest_id, timeout_us, (uint8_t*)&data_be16, 2);
271 : }
272 :
273 0 : bool AP_KDECAN_Driver::send_packet(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_us, const uint8_t *data, const uint8_t data_len)
274 : {
275 : // broadcast telemetry request frame
276 0 : const frame_id_t id {
277 : {
278 : .object_address = address,
279 : .destination_id = dest_id,
280 : .source_id = AUTOPILOT_NODE_ID,
281 : .priority = 0,
282 : .unused = 0
283 : }
284 0 : };
285 :
286 0 : AP_HAL::CANFrame frame = AP_HAL::CANFrame((id.value | AP_HAL::CANFrame::FlagEFF), data, data_len, false);
287 :
288 0 : return write_frame(frame, timeout_us);
289 : }
290 :
291 : // singleton instance
292 : AP_KDECAN *AP_KDECAN::_singleton;
293 :
294 : namespace AP {
295 83384328 : AP_KDECAN *kdecan()
296 : {
297 83384328 : return AP_KDECAN::get_singleton();
298 : }
299 : };
300 :
301 : #endif // AP_KDECAN_ENABLED
302 :
|
