forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
AC_Sprayer.cpp
204 lines (173 loc) · 6.2 KB
/
AC_Sprayer.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
#include "AC_Sprayer.h"
#if HAL_SPRAYER_ENABLED
#include <AP_AHRS/AP_AHRS.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <SRV_Channel/SRV_Channel.h>
extern const AP_HAL::HAL& hal;
// ------------------------------
const AP_Param::GroupInfo AC_Sprayer::var_info[] = {
// @Param: ENABLE
// @DisplayName: Sprayer enable/disable
// @Description: Allows you to enable (1) or disable (0) the sprayer
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO_FLAGS("ENABLE", 0, AC_Sprayer, _enabled, 0, AP_PARAM_FLAG_ENABLE),
// @Param: PUMP_RATE
// @DisplayName: Pump speed
// @Description: Desired pump speed when traveling 1m/s expressed as a percentage
// @Units: %
// @Range: 0 100
// @User: Standard
AP_GROUPINFO("PUMP_RATE", 1, AC_Sprayer, _pump_pct_1ms, AC_SPRAYER_DEFAULT_PUMP_RATE),
// @Param: SPINNER
// @DisplayName: Spinner rotation speed
// @Description: Spinner's rotation speed in PWM (a higher rate will disperse the spray over a wider area horizontally)
// @Units: ms
// @Range: 1000 2000
// @User: Standard
AP_GROUPINFO("SPINNER", 2, AC_Sprayer, _spinner_pwm, AC_SPRAYER_DEFAULT_SPINNER_PWM),
// @Param: SPEED_MIN
// @DisplayName: Speed minimum
// @Description: Speed minimum at which we will begin spraying
// @Units: cm/s
// @Range: 0 1000
// @User: Standard
AP_GROUPINFO("SPEED_MIN", 3, AC_Sprayer, _speed_min, AC_SPRAYER_DEFAULT_SPEED_MIN),
// @Param: PUMP_MIN
// @DisplayName: Pump speed minimum
// @Description: Minimum pump speed expressed as a percentage
// @Units: %
// @Range: 0 100
// @User: Standard
AP_GROUPINFO("PUMP_MIN", 4, AC_Sprayer, _pump_min_pct, AC_SPRAYER_DEFAULT_PUMP_MIN),
AP_GROUPEND
};
AC_Sprayer::AC_Sprayer()
{
if (_singleton) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("Too many sprayers");
#endif
return;
}
_singleton = this;
AP_Param::setup_object_defaults(this, var_info);
// check for silly parameter values
if (_pump_pct_1ms < 0.0f || _pump_pct_1ms > 100.0f) {
_pump_pct_1ms.set_and_save(AC_SPRAYER_DEFAULT_PUMP_RATE);
}
if (_spinner_pwm < 0) {
_spinner_pwm.set_and_save(AC_SPRAYER_DEFAULT_SPINNER_PWM);
}
// To-Do: ensure that the pump and spinner servo channels are enabled
}
/*
* Get the AP_Sprayer singleton
*/
AC_Sprayer *AC_Sprayer::_singleton;
AC_Sprayer *AC_Sprayer::get_singleton()
{
return _singleton;
}
void AC_Sprayer::run(const bool true_false)
{
// return immediately if no change
if (true_false == _flags.running) {
return;
}
// set flag indicate whether spraying is permitted:
// do not allow running to be set to true if we are currently not enabled
_flags.running = true_false && _enabled;
// turn off the pump and spinner servos if necessary
if (!_flags.running) {
stop_spraying();
}
}
void AC_Sprayer::stop_spraying()
{
SRV_Channels::set_output_limit(SRV_Channel::k_sprayer_pump, SRV_Channel::Limit::MIN);
SRV_Channels::set_output_limit(SRV_Channel::k_sprayer_spinner, SRV_Channel::Limit::MIN);
_flags.spraying = false;
}
/// update - adjust pwm of servo controlling pump speed according to the desired quantity and our horizontal speed
void AC_Sprayer::update()
{
// exit immediately if we are disabled or shouldn't be running
if (!_enabled || !running()) {
run(false);
return;
}
// exit immediately if the pump function has not been set-up for any servo
if (!SRV_Channels::function_assigned(SRV_Channel::k_sprayer_pump)) {
return;
}
// get horizontal velocity
Vector3f velocity;
if (!AP::ahrs().get_velocity_NED(velocity)) {
// treat unknown velocity as zero which should lead to pump stopping
// velocity will already be zero but this avoids a coverity warning
velocity.zero();
}
float ground_speed = velocity.xy().length() * 100.0;
// get the current time
const uint32_t now = AP_HAL::millis();
bool should_be_spraying = _flags.spraying;
// check our speed vs the minimum
if (ground_speed >= _speed_min) {
// if we are not already spraying
if (!_flags.spraying) {
// set the timer if this is the first time we've surpassed the min speed
if (_speed_over_min_time == 0) {
_speed_over_min_time = now;
}else{
// check if we've been over the speed long enough to engage the sprayer
if((now - _speed_over_min_time) > AC_SPRAYER_DEFAULT_TURN_ON_DELAY) {
should_be_spraying = true;
_speed_over_min_time = 0;
}
}
}
// reset the speed under timer
_speed_under_min_time = 0;
} else {
// we are under the min speed.
if (_flags.spraying) {
// set the timer if this is the first time we've dropped below the min speed
if (_speed_under_min_time == 0) {
_speed_under_min_time = now;
}else{
// check if we've been over the speed long enough to engage the sprayer
if((now - _speed_under_min_time) > AC_SPRAYER_DEFAULT_SHUT_OFF_DELAY) {
should_be_spraying = false;
_speed_under_min_time = 0;
}
}
}
// reset the speed over timer
_speed_over_min_time = 0;
}
// if testing pump output speed as if traveling at 1m/s
if (_flags.testing) {
ground_speed = 100.0f;
should_be_spraying = true;
}
// if spraying or testing update the pump servo position
if (should_be_spraying) {
float pos = ground_speed * _pump_pct_1ms;
pos = MAX(pos, 100 *_pump_min_pct); // ensure min pump speed
pos = MIN(pos,10000); // clamp to range
SRV_Channels::move_servo(SRV_Channel::k_sprayer_pump, pos, 0, 10000);
SRV_Channels::set_output_pwm(SRV_Channel::k_sprayer_spinner, _spinner_pwm);
_flags.spraying = true;
} else {
stop_spraying();
}
}
namespace AP {
AC_Sprayer *sprayer()
{
return AC_Sprayer::get_singleton();
}
};
#endif // HAL_SPRAYER_ENABLED