Line data Source code
1 : #include "AP_Winch.h"
2 :
3 : #if AP_WINCH_ENABLED
4 :
5 : #include <GCS_MAVLink/GCS.h>
6 : #include "AP_Winch_PWM.h"
7 : #include "AP_Winch_Daiwa.h"
8 :
9 : extern const AP_HAL::HAL& hal;
10 :
11 : const AP_Param::GroupInfo AP_Winch::var_info[] = {
12 : // 0 was ENABLE
13 :
14 : // @Param: _TYPE
15 : // @DisplayName: Winch Type
16 : // @Description: Winch Type
17 : // @User: Standard
18 : // @Values: 0:None, 1:PWM, 2:Daiwa
19 : AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Winch, config.type, (int8_t)WinchType::NONE, AP_PARAM_FLAG_ENABLE),
20 :
21 : // @Param: _RATE_MAX
22 : // @DisplayName: Winch deploy or retract rate maximum
23 : // @Description: Winch deploy or retract rate maximum. Set to maximum rate with no load.
24 : // @User: Standard
25 : // @Range: 0 10
26 : // @Units: m/s
27 : AP_GROUPINFO("_RATE_MAX", 2, AP_Winch, config.rate_max, 1.0f),
28 :
29 : // @Param: _POS_P
30 : // @DisplayName: Winch control position error P gain
31 : // @Description: Winch control position error P gain
32 : // @Range: 0.01 10.0
33 : // @User: Standard
34 : AP_GROUPINFO("_POS_P", 3, AP_Winch, config.pos_p, 1.0f),
35 :
36 : // @Param: _OPTIONS
37 : // @DisplayName: Winch options
38 : // @Description: Winch options
39 : // @Bitmask: 0:Spin freely on startup, 1:Verbose output, 2:Retry if stuck (Daiwa only)
40 : // @User: Standard
41 : AP_GROUPINFO("_OPTIONS", 4, AP_Winch, config.options, 7.0f),
42 :
43 : // 4 was _RATE_PID
44 :
45 : AP_GROUPEND
46 : };
47 :
48 1001 : AP_Winch::AP_Winch()
49 : {
50 1001 : if (_singleton) {
51 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
52 0 : AP_HAL::panic("Too many winches");
53 : #endif
54 : return;
55 : }
56 1001 : _singleton = this;
57 :
58 1001 : AP_Param::setup_object_defaults(this, var_info);
59 : }
60 :
61 : // indicate whether this module is enabled
62 4529756 : bool AP_Winch::enabled() const
63 : {
64 4529756 : return ((config.type > 0) && (backend != nullptr));
65 : }
66 :
67 : // true if winch is healthy
68 104 : bool AP_Winch::healthy() const
69 : {
70 104 : if (backend != nullptr) {
71 102 : return backend->healthy();
72 : }
73 2 : return false;
74 : }
75 :
76 1001 : void AP_Winch::init()
77 : {
78 1001 : switch ((WinchType)config.type.get()) {
79 999 : case WinchType::NONE:
80 999 : break;
81 : #if AP_WINCH_PWM_ENABLED
82 2 : case WinchType::PWM:
83 2 : backend = NEW_NOTHROW AP_Winch_PWM(config);
84 2 : break;
85 : #endif
86 : #if AP_WINCH_DAIWA_ENABLED
87 0 : case WinchType::DAIWA:
88 0 : backend = NEW_NOTHROW AP_Winch_Daiwa(config);
89 0 : break;
90 : #endif
91 0 : default:
92 0 : break;
93 : }
94 1001 : if (backend != nullptr) {
95 2 : backend->init();
96 :
97 : // initialise control mode
98 2 : if (backend->option_enabled(Options::SpinFreelyOnStartup)) {
99 2 : relax();
100 : } else {
101 0 : set_desired_rate(0);
102 : }
103 : }
104 1001 : }
105 :
106 : // release specified length of cable (in meters)
107 2 : void AP_Winch::release_length(float length)
108 : {
109 2 : if (backend == nullptr) {
110 0 : return;
111 : }
112 2 : config.length_desired = backend->get_current_length() + length;
113 2 : config.control_mode = ControlMode::POSITION;
114 :
115 : // display verbose output to user
116 2 : if (backend->option_enabled(Options::VerboseOutput)) {
117 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Winch: %s %4.1fm to %4.1fm", is_negative(length) ? "raising" : "lowering", (double)fabsf(length), (double)config.length_desired);
118 : }
119 : }
120 :
121 : // deploy line at specified speed in m/s (+ve deploys line, -ve retracts line, 0 stops)
122 4 : void AP_Winch::set_desired_rate(float rate)
123 : {
124 4 : config.rate_desired = constrain_float(rate, -get_rate_max(), get_rate_max());
125 4 : config.control_mode = ControlMode::RATE;
126 4 : }
127 :
128 : // send status to ground station
129 385952 : void AP_Winch::send_status(const GCS_MAVLINK &channel)
130 : {
131 385952 : if (backend != nullptr) {
132 510 : backend->send_status(channel);
133 : }
134 385952 : }
135 :
136 : // returns true if pre arm checks have passed
137 89989 : bool AP_Winch::pre_arm_check(char *failmsg, uint8_t failmsg_len) const
138 : {
139 : // succeed if winch is disabled
140 89989 : if ((WinchType)config.type.get() == WinchType::NONE) {
141 89885 : return true;
142 : }
143 :
144 : // fail if unhealthy
145 104 : if (!healthy()) {
146 2 : hal.util->snprintf(failmsg, failmsg_len, "winch unhealthy");
147 2 : return false;
148 : }
149 :
150 102 : return true;
151 : }
152 :
153 : // update - should be called at at least 10hz
154 : #define PASS_TO_BACKEND(function_name) \
155 : void AP_Winch::function_name() \
156 : { \
157 : if (!enabled()) { \
158 : return; \
159 : } \
160 : if (backend != nullptr) { \
161 : backend->function_name(); \
162 : } \
163 : }
164 :
165 3800947 : PASS_TO_BACKEND(update)
166 : #if HAL_LOGGING_ENABLED
167 728799 : PASS_TO_BACKEND(write_log)
168 : #endif
169 :
170 : #undef PASS_TO_BACKEND
171 :
172 : /*
173 : * Get the AP_Winch singleton
174 : */
175 : AP_Winch *AP_Winch::_singleton;
176 475941 : AP_Winch *AP_Winch::get_singleton()
177 : {
178 475941 : return _singleton;
179 : }
180 :
181 : namespace AP {
182 :
183 475941 : AP_Winch *winch()
184 : {
185 475941 : return AP_Winch::get_singleton();
186 : }
187 :
188 : };
189 :
190 : #endif // AP_WINCH_ENABLED
|
