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 : /* 17 : The Daiwa winch is produced by a Japanese company called Okaya. 18 : There are two PWM controls supported: 19 : - the rate control for releasing (high PWM) or retracting (low PWM) the line 20 : - the clutch control has three settings: 21 : - released (high PWM) lets the winch spin freely 22 : - engaged soft (middle PWM) allows the rate control to work but it may slip 23 : if too much tension is required. This driver does not use this setting. 24 : - engaged hard (low PWM) allows the rate control to work regardless of tension. 25 : A telemetry output from the winch is connected to the autopilot and provides 26 : the amount of line released, tension, clutch setting, etc. 27 : */ 28 : 29 : #pragma once 30 : 31 : #include <AP_Winch/AP_Winch_Backend.h> 32 : 33 : #if AP_WINCH_DAIWA_ENABLED 34 : 35 : class AP_Winch_Daiwa : public AP_Winch_Backend { 36 : public: 37 : 38 : using AP_Winch_Backend::AP_Winch_Backend; 39 : 40 : // true if winch is healthy 41 : bool healthy() const override; 42 : 43 : // initialise the winch 44 : void init() override; 45 : 46 : // read telemetry from the winch and output controls 47 : void update() override; 48 : 49 : // returns current length of line deployed 50 0 : float get_current_length() const override { return latest.line_length; } 51 : 52 : // send status to ground station 53 : void send_status(const GCS_MAVLINK &channel) override; 54 : 55 : #if HAL_LOGGING_ENABLED 56 : // write log 57 : void write_log() override; 58 : #endif 59 : 60 : private: 61 : 62 : // read incoming data from winch and update intermediate and latest structures 63 : void read_data_from_winch(); 64 : 65 : // update pwm outputs to control winch 66 : void control_winch(); 67 : 68 : // returns the rate which may be modified to unstick the winch 69 : // if the winch stops, the rate is temporarily set to zero 70 : // now_ms should be set to the current system time 71 : // rate should be the rate used to calculate the final PWM output to the winch 72 : float get_stuck_protected_rate(uint32_t now_ms, float rate); 73 : 74 : static const uint8_t buff_len_max = 20; // buffer maximum length 75 : static const int16_t output_dz = 100; // output deadzone in scale of -1000 to +1000 76 : static constexpr float line_length_correction_factor = 0.003333f; // convert winch counter to meters 77 : 78 : AP_HAL::UARTDriver *uart; 79 : char buff[buff_len_max]; // buffer holding latest data from winch 80 : uint8_t buff_len; // number of bytes in buff 81 : 82 : // winch data 83 : // latest holds most recent complete data received 84 : // intermediate holds partial results currently being processed 85 : struct WinchData { 86 : uint32_t time_ms; // winch system time in milliseconds 87 : float line_length; // length of line released in meters 88 : uint16_t tension_uncorrected; // uncorrected tension in grams (0 to 1024) 89 : uint16_t tension_corrected; // corrected tension in grams (0 to 1024) 90 : bool thread_end; // true if end of thread has been detected 91 : uint8_t moving; // 0:stopped, 1:retracting line, 2:extending line, 3:clutch engaged, 4:zero reset 92 : uint8_t clutch; // 0:clutch off, 1:clutch engaged weakly, 2:clutch engaged strongly, motor can spin freely 93 : uint8_t speed_pct; // speed motor is moving as a percentage 94 : float voltage; // battery voltage (in voltes) 95 : float current; // current draw (in amps) 96 : float pcb_temp; // PCB temp in C 97 : float motor_temp; // motor temp in C 98 : } latest, intermediate; 99 : uint32_t data_update_ms; // system time that latest was last updated 100 : uint32_t control_update_ms; // last time control_winch was called 101 : 102 : // parsing state 103 : enum class ParseState : uint8_t { 104 : WAITING_FOR_TIME = 0, 105 : WAITING_FOR_SPOOL, 106 : WAITING_FOR_TENSION1, 107 : WAITING_FOR_TENSION2, 108 : WAITING_FOR_THREAD_END, 109 : WAITING_FOR_MOVING, 110 : WAITING_FOR_CLUTCH, 111 : WAITING_FOR_SPEED, 112 : WAITING_FOR_VOLTAGE, 113 : WAITING_FOR_CURRENT, 114 : WAITING_FOR_PCB_TEMP, 115 : WAITING_FOR_MOTOR_TEMP 116 : } parse_state; 117 : 118 : // update user with changes to winch state via send text messages 119 : static const char* send_text_prefix;// send text prefix string to reduce flash cost 120 : void update_user(); 121 : struct { 122 : uint32_t last_ms; // system time of last update to user 123 : bool healthy; // latest reported health 124 : float line_length; 125 : bool thread_end; // true if end of thread has been detected 126 : uint8_t moving; // 0:stopped, 1:retracting line, 2:extending line, 3:clutch engaged, 4:zero reset 127 : uint8_t clutch; // 0:clutch off, 1:clutch engaged weakly, 2:clutch engaged strongly, motor can spin freely 128 : } user_update; 129 : 130 : // stuck protection 131 : struct { 132 : uint32_t last_update_ms; // system time that stuck protection was last called 133 : uint32_t stuck_start_ms; // system time that winch became stuck (0 if not stuck) 134 : bool user_notified; // true if user has been notified that winch is stuck 135 : } stuck_protection; 136 : }; 137 : 138 : #endif // AP_WINCH_DAIWA_ENABLED
