Line data Source code
1 : #include <AP_Winch/AP_Winch_Daiwa.h>
2 :
3 : #if AP_WINCH_DAIWA_ENABLED
4 :
5 : #include <AP_Logger/AP_Logger.h>
6 : #include <GCS_MAVLink/GCS.h>
7 : #include <SRV_Channel/SRV_Channel.h>
8 :
9 : #define AP_WINCH_DAIWA_STUCK_TIMEOUT_MS 1000 // winch is considered stuck if unmoving for this many milliseconds
10 : #define AP_WINCH_DAIWA_STUCK_CENTER_MS 1000 // stuck protection outputs zero rate for this many milliseconds
11 : #define AP_WINCH_DAIWA_STUCK_LENGTH_MIN 0.1 // stuck protection active when line length is more than this many meters
12 : #define AP_WINCH_DAIWA_STUCK_RATE_MIN 0.2 // stuck protection active when desired rate is at least this value (+ve or -ve)
13 :
14 : extern const AP_HAL::HAL& hal;
15 :
16 : const char* AP_Winch_Daiwa::send_text_prefix = "Winch:";
17 :
18 : // true if winch is healthy
19 0 : bool AP_Winch_Daiwa::healthy() const
20 : {
21 : // healthy if we have received data within 3 seconds
22 0 : return (AP_HAL::millis() - data_update_ms < 3000);
23 : }
24 :
25 0 : void AP_Winch_Daiwa::init()
26 : {
27 : // call superclass init
28 0 : AP_Winch_Backend::init();
29 :
30 : // initialise serial connection to winch
31 0 : const AP_SerialManager &serial_manager = AP::serialmanager();
32 0 : uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Winch, 0);
33 0 : }
34 :
35 0 : void AP_Winch_Daiwa::update()
36 : {
37 : // return immediately if no servo is assigned to control the winch
38 0 : if (!SRV_Channels::function_assigned(SRV_Channel::k_winch)) {
39 0 : return;
40 : }
41 :
42 : // read latest data from winch
43 0 : read_data_from_winch();
44 :
45 : // read pilot input
46 0 : read_pilot_desired_rate();
47 :
48 : // send outputs to winch
49 0 : control_winch();
50 :
51 : // update_user
52 0 : update_user();
53 : }
54 :
55 : //send generator status
56 0 : void AP_Winch_Daiwa::send_status(const GCS_MAVLINK &channel)
57 : {
58 : // prepare status bitmask
59 0 : uint32_t status_bitmask = 0;
60 0 : if (healthy()) {
61 0 : status_bitmask |= MAV_WINCH_STATUS_HEALTHY;
62 : }
63 0 : if (latest.thread_end) {
64 0 : status_bitmask |= MAV_WINCH_STATUS_FULLY_RETRACTED;
65 : }
66 0 : if (latest.moving > 0) {
67 0 : status_bitmask |= MAV_WINCH_STATUS_MOVING;
68 : }
69 0 : if (latest.clutch > 0) {
70 0 : status_bitmask |= MAV_WINCH_STATUS_CLUTCH_ENGAGED;
71 : }
72 :
73 : // convert speed percentage to absolute speed
74 0 : const float speed_ms = fabsf(config.rate_max) * (float)latest.speed_pct * 0.01f;
75 :
76 : // send status
77 0 : mavlink_msg_winch_status_send(
78 : channel.get_chan(),
79 : AP_HAL::micros64(),
80 : latest.line_length,
81 : speed_ms,
82 0 : (float)latest.tension_corrected * 0.01f,
83 : latest.voltage,
84 : latest.current,
85 0 : latest.motor_temp,
86 : status_bitmask);
87 0 : }
88 :
89 : #if HAL_LOGGING_ENABLED
90 : // write log
91 0 : void AP_Winch_Daiwa::write_log()
92 : {
93 0 : AP::logger().Write_Winch(
94 0 : healthy(),
95 0 : latest.thread_end,
96 0 : latest.moving,
97 0 : latest.clutch,
98 0 : (uint8_t)config.control_mode,
99 0 : config.length_desired,
100 0 : get_current_length(),
101 0 : config.rate_desired,
102 0 : latest.tension_corrected,
103 : latest.voltage,
104 0 : constrain_int16(latest.motor_temp, INT8_MIN, INT8_MAX));
105 0 : }
106 : #endif
107 :
108 : // read incoming data from winch and update intermediate and latest structures
109 0 : void AP_Winch_Daiwa::read_data_from_winch()
110 : {
111 : // return immediately if serial port is not configured
112 0 : if (uart == nullptr) {
113 0 : return;
114 : }
115 :
116 : // read any available characters from serial port and send to GCS
117 0 : int16_t nbytes = uart->available();
118 0 : while (nbytes-- > 0) {
119 0 : int16_t b = uart->read();
120 :
121 0 : if ((b >= '0' && b <= '9') || (b >= 'A' && b <= 'F') || (b >= 'a' && b <= 'f')) {
122 : // add digits to buffer
123 0 : buff[buff_len] = b;
124 0 : buff_len++;
125 0 : if (buff_len >= ARRAY_SIZE(buff)) {
126 0 : buff_len = 0;
127 0 : parse_state = ParseState::WAITING_FOR_TIME;
128 : }
129 0 : } else if (b == ',' || b == '\r') {
130 : // comma or carriage return signals end of current value
131 : // parse number received and empty buffer
132 0 : buff[buff_len] = '\0';
133 0 : long int value = (int32_t)strtol(buff, nullptr, 16);
134 0 : buff_len = 0;
135 0 : switch (parse_state) {
136 0 : case ParseState::WAITING_FOR_TIME:
137 0 : intermediate.time_ms = (uint32_t)value;
138 0 : parse_state = ParseState::WAITING_FOR_SPOOL;
139 0 : break;
140 0 : case ParseState::WAITING_FOR_SPOOL:
141 0 : intermediate.line_length = (int32_t)value * line_length_correction_factor;
142 0 : parse_state = ParseState::WAITING_FOR_TENSION1;
143 0 : break;
144 0 : case ParseState::WAITING_FOR_TENSION1:
145 0 : intermediate.tension_uncorrected = (uint16_t)value;
146 0 : parse_state = ParseState::WAITING_FOR_TENSION2;
147 0 : break;
148 0 : case ParseState::WAITING_FOR_TENSION2:
149 0 : intermediate.tension_corrected = (uint16_t)value;
150 0 : parse_state = ParseState::WAITING_FOR_THREAD_END;
151 0 : break;
152 0 : case ParseState::WAITING_FOR_THREAD_END:
153 0 : intermediate.thread_end = (value > 0);
154 0 : parse_state = ParseState::WAITING_FOR_MOVING;
155 0 : break;
156 0 : case ParseState::WAITING_FOR_MOVING:
157 0 : intermediate.moving = constrain_int32(value, 0, UINT8_MAX);
158 0 : parse_state = ParseState::WAITING_FOR_CLUTCH;
159 0 : break;
160 0 : case ParseState::WAITING_FOR_CLUTCH:
161 0 : intermediate.clutch = constrain_int32(value, 0, UINT8_MAX);
162 0 : parse_state = ParseState::WAITING_FOR_SPEED;
163 0 : break;
164 0 : case ParseState::WAITING_FOR_SPEED:
165 0 : intermediate.speed_pct = constrain_int32(value, 0, UINT8_MAX);
166 0 : parse_state = ParseState::WAITING_FOR_VOLTAGE;
167 0 : break;
168 0 : case ParseState::WAITING_FOR_VOLTAGE:
169 0 : intermediate.voltage = (float)value * 0.1f;
170 0 : parse_state = ParseState::WAITING_FOR_CURRENT;
171 0 : break;
172 0 : case ParseState::WAITING_FOR_CURRENT:
173 0 : intermediate.current = (float)value * 0.1f;
174 0 : parse_state = ParseState::WAITING_FOR_PCB_TEMP;
175 0 : break;
176 0 : case ParseState::WAITING_FOR_PCB_TEMP:
177 0 : intermediate.pcb_temp = (float)value * 0.1f;
178 0 : parse_state = ParseState::WAITING_FOR_MOTOR_TEMP;
179 0 : break;
180 0 : case ParseState::WAITING_FOR_MOTOR_TEMP:
181 0 : intermediate.motor_temp = (float)value * 0.1f;
182 : // successfully parsed a complete message
183 0 : latest = intermediate;
184 0 : data_update_ms = AP_HAL::millis();
185 0 : parse_state = ParseState::WAITING_FOR_TIME;
186 0 : break;
187 : }
188 0 : } else {
189 : // line feed or unexpected characters
190 0 : buff_len = 0;
191 0 : parse_state = ParseState::WAITING_FOR_TIME;
192 : }
193 : }
194 : }
195 :
196 : // update pwm outputs to control winch
197 0 : void AP_Winch_Daiwa::control_winch()
198 : {
199 0 : const uint32_t now_ms = AP_HAL::millis();
200 0 : float dt = (now_ms - control_update_ms) * 0.001f;
201 0 : if (dt > 1.0f) {
202 0 : dt = 0.0f;
203 : }
204 0 : control_update_ms = now_ms;
205 :
206 : // if real doing any control output trim value
207 0 : if (config.control_mode == AP_Winch::ControlMode::RELAXED) {
208 : // if not doing any control output release clutch and move winch to trim
209 0 : SRV_Channels::set_output_limit(SRV_Channel::k_winch_clutch, SRV_Channel::Limit::MAX);
210 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_winch, 0);
211 :
212 : // rate used for acceleration limiting reset to zero
213 0 : set_previous_rate(0.0f);
214 0 : return;
215 : }
216 :
217 : // release clutch
218 0 : SRV_Channels::set_output_limit(SRV_Channel::k_winch_clutch, SRV_Channel::Limit::MIN);
219 :
220 : // if doing position control, calculate position error to desired rate
221 0 : if ((config.control_mode == AP_Winch::ControlMode::POSITION) && healthy()) {
222 0 : float position_error = config.length_desired - latest.line_length;
223 0 : config.rate_desired = constrain_float(position_error * config.pos_p, -config.rate_max, config.rate_max);
224 : }
225 :
226 : // apply acceleration limited to rate
227 0 : float rate_limited = get_rate_limited_by_accel(config.rate_desired, dt);
228 :
229 : // apply stuck protection to rate
230 0 : rate_limited = get_stuck_protected_rate(now_ms, rate_limited);
231 :
232 : // use linear interpolation to calculate output to move winch at desired rate
233 0 : float scaled_output = 0;
234 0 : if (!is_zero(rate_limited)) {
235 0 : scaled_output = linear_interpolate(output_dz, 1000, fabsf(rate_limited), 0, config.rate_max) * (is_positive(rate_limited) ? 1.0f : -1.0f);
236 : }
237 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_winch, scaled_output);
238 : }
239 :
240 : // returns the rate which may be modified to unstick the winch
241 : // if the winch stops, the rate is temporarily set to zero
242 : // now_ms should be set to the current system time
243 : // rate should be the rate used to calculate the final PWM output to the winch
244 0 : float AP_Winch_Daiwa::get_stuck_protected_rate(uint32_t now_ms, float rate)
245 : {
246 : // exit immediately if stuck protection disabled
247 0 : if (!option_enabled(AP_Winch::Options::RetryIfStuck)) {
248 0 : return rate;
249 : }
250 :
251 : // check for timeout
252 0 : bool timeout = (now_ms - stuck_protection.last_update_ms) > 1000;
253 0 : stuck_protection.last_update_ms = now_ms;
254 :
255 : // check if winch is nearly fully pulled in
256 0 : const bool near_thread_start = (latest.line_length < AP_WINCH_DAIWA_STUCK_LENGTH_MIN) && is_negative(rate);
257 :
258 : // check if rate is near zero (winch may not move with very low desired rates)
259 0 : const bool rate_near_zero = fabsf(rate) < AP_WINCH_DAIWA_STUCK_RATE_MIN;
260 :
261 : // return rate unchanged if this protection has not been called recently or winch is unhealthy
262 : // or if winch is moving, desired rate is near zero or winch has stopped at thread start or thread end
263 0 : if (timeout || !healthy() || latest.moving || rate_near_zero || near_thread_start || latest.thread_end) {
264 : // notify user when winch becomes unstuck
265 0 : if (option_enabled(AP_Winch::Options::VerboseOutput) && (stuck_protection.stuck_start_ms != 0) && (stuck_protection.user_notified)) {
266 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s unstuck", send_text_prefix);
267 : }
268 : // reset stuck protection state
269 0 : stuck_protection.stuck_start_ms = 0;
270 0 : return rate;
271 : }
272 :
273 : // winch is healthy, with non-zero requested rate but not moving
274 : // record when winch became stuck
275 0 : if (stuck_protection.stuck_start_ms == 0) {
276 0 : stuck_protection.stuck_start_ms = now_ms;
277 0 : stuck_protection.user_notified = false;
278 : }
279 :
280 : // if stuck for between 1 to 2 seconds return zero rate
281 0 : const uint32_t stuck_time_ms = (now_ms - stuck_protection.stuck_start_ms);
282 0 : if (stuck_time_ms > AP_WINCH_DAIWA_STUCK_TIMEOUT_MS) {
283 : // notify user
284 0 : if (!stuck_protection.user_notified) {
285 0 : stuck_protection.user_notified = true;
286 0 : if (option_enabled(AP_Winch::Options::VerboseOutput)) {
287 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s stuck", send_text_prefix);
288 : }
289 : }
290 :
291 : // return zero rate for 1 second
292 0 : if (stuck_time_ms <= (AP_WINCH_DAIWA_STUCK_TIMEOUT_MS+AP_WINCH_DAIWA_STUCK_CENTER_MS)) {
293 0 : return 0;
294 : }
295 :
296 : // rate has been set to zero for 1 sec so release and restart stuck detection
297 0 : stuck_protection.stuck_start_ms = 0;
298 :
299 : // rate used for acceleration limiting also reset to zero
300 0 : set_previous_rate(0.0f);
301 :
302 : // update user
303 0 : if (option_enabled(AP_Winch::Options::VerboseOutput)) {
304 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s retrying", send_text_prefix);
305 : }
306 : }
307 :
308 : // give winch more time to start moving
309 0 : return rate;
310 : }
311 :
312 : // update user with changes to winch state via send text messages
313 0 : void AP_Winch_Daiwa::update_user()
314 : {
315 : // exit immediately if verbose output disabled
316 0 : if (!option_enabled(AP_Winch::Options::VerboseOutput)) {
317 0 : return;
318 : }
319 :
320 : // send updates at no more than 2hz
321 0 : uint32_t now_ms = AP_HAL::millis();
322 0 : if (now_ms - user_update.last_ms < 500) {
323 0 : return;
324 : }
325 0 : bool update_sent = false;
326 :
327 : // health change
328 0 : const bool now_healthy = healthy();
329 0 : if (user_update.healthy != now_healthy) {
330 0 : user_update.healthy = now_healthy;
331 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %shealthy", send_text_prefix, now_healthy ? "" : "not ");
332 0 : update_sent = true;
333 : }
334 :
335 : // thread end
336 0 : if (latest.thread_end && (user_update.thread_end != latest.thread_end)) {
337 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s thread end", send_text_prefix);
338 0 : update_sent = true;
339 : }
340 0 : user_update.thread_end = latest.thread_end;
341 :
342 : // moving state
343 0 : if (user_update.moving != latest.moving) {
344 : // 0:stopped, 1:retracting line, 2:extending line, 3:clutch engaged, 4:zero reset
345 : static const char* moving_str[] = {"stopped", "raising", "lowering", "free spinning", "zero reset"};
346 0 : if (latest.moving < ARRAY_SIZE(moving_str)) {
347 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_text_prefix, moving_str[latest.moving]);
348 : } else {
349 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s move state unknown", send_text_prefix);
350 : }
351 0 : update_sent = true;
352 : }
353 0 : user_update.moving = latest.moving;
354 :
355 : // clutch state
356 0 : if (user_update.clutch != latest.clutch) {
357 : // 0:clutch off, 1:clutch engaged weakly, 2:clutch engaged strongly, motor can spin freely
358 : static const char* clutch_str[] = {"off", "weak", "strong (free)"};
359 0 : if (user_update.clutch < ARRAY_SIZE(clutch_str)) {
360 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s clutch %s", send_text_prefix, clutch_str[latest.moving]);
361 : } else {
362 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s clutch state unknown", send_text_prefix);
363 : }
364 0 : update_sent = true;
365 : }
366 0 : user_update.clutch = latest.clutch;
367 :
368 : // length in meters
369 0 : const float latest_line_length_rounded = roundf(latest.line_length);
370 0 : if (!is_equal(user_update.line_length, latest_line_length_rounded)) {
371 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %dm", send_text_prefix, (int)latest_line_length_rounded);
372 0 : update_sent = true;
373 : }
374 0 : user_update.line_length = latest_line_length_rounded;
375 :
376 : // record time message last sent to user
377 0 : if (update_sent) {
378 0 : user_update.last_ms = now_ms;
379 : }
380 : }
381 :
382 : #endif // AP_WINCH_DAIWA_ENABLED
|
