123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200 |
- #include "AC_Sprayer.h"
- #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[] = {
-
-
-
-
-
- AP_GROUPINFO_FLAGS("ENABLE", 0, AC_Sprayer, _enabled, 0, AP_PARAM_FLAG_ENABLE),
-
-
-
-
-
-
- AP_GROUPINFO("PUMP_RATE", 1, AC_Sprayer, _pump_pct_1ms, AC_SPRAYER_DEFAULT_PUMP_RATE),
-
-
-
-
-
-
- AP_GROUPINFO("SPINNER", 2, AC_Sprayer, _spinner_pwm, AC_SPRAYER_DEFAULT_SPINNER_PWM),
-
-
-
-
-
-
- AP_GROUPINFO("SPEED_MIN", 3, AC_Sprayer, _speed_min, AC_SPRAYER_DEFAULT_SPEED_MIN),
-
-
-
-
-
-
- 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);
-
- 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);
- }
-
- }
- AC_Sprayer *AC_Sprayer::_singleton;
- AC_Sprayer *AC_Sprayer::get_singleton()
- {
- return _singleton;
- }
- void AC_Sprayer::run(const bool true_false)
- {
-
- if (true_false == _flags.running) {
- return;
- }
-
-
- _flags.running = true_false && _enabled;
-
- if (!_flags.running) {
- stop_spraying();
- }
- }
- void AC_Sprayer::stop_spraying()
- {
- SRV_Channels::set_output_limit(SRV_Channel::k_sprayer_pump, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
- SRV_Channels::set_output_limit(SRV_Channel::k_sprayer_spinner, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
- _flags.spraying = false;
- }
- void AC_Sprayer::update()
- {
-
- if (!_enabled || !running()) {
- run(false);
- return;
- }
-
- if (!SRV_Channels::function_assigned(SRV_Channel::k_sprayer_pump)) {
- return;
- }
-
- Vector3f velocity;
- if (!AP::ahrs().get_velocity_NED(velocity)) {
-
-
- velocity.zero();
- }
- float ground_speed = norm(velocity.x * 100.0f, velocity.y * 100.0f);
-
- const uint32_t now = AP_HAL::millis();
- bool should_be_spraying = _flags.spraying;
-
- if (ground_speed >= _speed_min) {
-
- if (!_flags.spraying) {
-
- if (_speed_over_min_time == 0) {
- _speed_over_min_time = now;
- }else{
-
- if((now - _speed_over_min_time) > AC_SPRAYER_DEFAULT_TURN_ON_DELAY) {
- should_be_spraying = true;
- _speed_over_min_time = 0;
- }
- }
- }
-
- _speed_under_min_time = 0;
- } else {
-
- if (_flags.spraying) {
-
- if (_speed_under_min_time == 0) {
- _speed_under_min_time = now;
- }else{
-
- if((now - _speed_under_min_time) > AC_SPRAYER_DEFAULT_SHUT_OFF_DELAY) {
- should_be_spraying = false;
- _speed_under_min_time = 0;
- }
- }
- }
-
- _speed_over_min_time = 0;
- }
-
- if (_flags.testing) {
- ground_speed = 100.0f;
- should_be_spraying = true;
- }
-
- if (should_be_spraying) {
- float pos = ground_speed * _pump_pct_1ms;
- pos = MAX(pos, 100 *_pump_min_pct);
- pos = MIN(pos,10000);
- 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();
- }
- };
|