AC_Sprayer: first implementation

libraries/AC_Sprayer/AC_Sprayer.cpp

@@ -0,0 +1,158 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+#include <AC_Sprayer.h>
+
+extern const AP_HAL::HAL& hal;
+
+// ------------------------------
+
+const AP_Param::GroupInfo AC_Sprayer::var_info[] PROGMEM = {
+    // @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("ENABLE",      0,  AC_Sprayer, _enabled, 0),
+
+    // @Param: PUMP_RATE
+    // @DisplayName: Pump speed
+    // @Description: Desired pump speed when travelling 1m/s expressed as a percentage
+    // @Units: percentage
+    // @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),
+
+    AP_GROUPEND
+};
+
+/// Default constructor.
+AC_Sprayer::AC_Sprayer(AP_InertialNav* inav) :
+    _inav(inav),
+    _spraying(false),
+    _speed_over_min_time(0),
+    _speed_under_min_time(0)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+
+    // check for silly parameter values
+    if (_pump_pct_1ms < 0 || _pump_pct_1ms > 100) {
+        _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
+}
+
+/// enable - allows fence to be enabled/disabled.  Note: this does not update the eeprom saved value
+void AC_Sprayer::enable(bool true_false)
+{
+    // return immediately if no change
+    if (true_false == _enabled) {
+        return;
+    }
+
+    // set enabled/disabled parameter (in memory only)
+    _enabled = true_false;
+
+    // turn off the pump and spinner servos if necessary
+    if (!_enabled) {
+        // send output to pump channel
+        // To-Do: change 0 below to radio_min of pump servo
+        RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_sprayer_pump);
+
+        // send output to spinner channel
+        // To-Do: change 0 below to radio_min of spinner servo
+        RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_sprayer_spinner);
+    }
+}
+
+/// update - adjust pwm of servo controlling pump speed according to the desired quantity and our horizontal speed
+void
+AC_Sprayer::update()
+{
+    uint32_t now;
+    Vector3f velocity;
+    float ground_speed;
+
+    // exit immediately if we are disabled (perhaps set pwm values back to defaults)
+    if (!_enabled) {
+        return;
+    }
+
+    // exit immediately if the pump function has not been set-up for any servo
+    if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_sprayer_pump)) {
+        return;
+    }
+
+    // get horizontal velocity
+    velocity = _inav->get_velocity();
+    ground_speed = pythagorous2(velocity.x,velocity.y);
+
+    // get the current time
+    now = hal.scheduler->millis();
+
+    // check our speed vs the minimum
+    if (ground_speed >= _speed_min) {
+        // if we are not already spraying
+        if (!_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) {
+                    _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 we are spraying
+        if (_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) {
+                    _spraying = false;
+                    _speed_under_min_time = 0;
+                }
+            }
+        }
+        // reset the speed over timer
+        _speed_over_min_time = 0;
+    }
+
+    // if spraying update the pump servo position
+    if (_spraying) {
+        RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, min(ground_speed * _pump_pct_1ms,10000),0,10000);
+    }else{
+        // ensure sprayer is off
+        RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_sprayer_pump);
+    }
+
+    // send output to spinner servo
+    RC_Channel_aux::set_radio(RC_Channel_aux::k_sprayer_spinner, _spinner_pwm);
+}

libraries/AC_Sprayer/AC_Sprayer.h

@@ -0,0 +1,62 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file	AC_Sprayer.h
+/// @brief	Crop sprayer library
+
+#ifndef AC_SPRAYER_H
+#define AC_SPRAYER_H
+
+#include <inttypes.h>
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <AP_Math.h>
+#include <RC_Channel.h>
+#include <AP_InertialNav.h>     // Inertial Navigation library
+
+#define AC_SPRAYER_DEFAULT_PUMP_RATE        0       // default quantity of spray per meter travelled
+#define AC_SPRAYER_DEFAULT_SPINNER_PWM      1300    // default speed of spinner (higher means spray is throw further horizontally
+#define AC_SPRAYER_DEFAULT_SPEED_MIN        100     // we must be travelling at least 1m/s to begin spraying
+#define AC_SPRAYER_DEFAULT_TURN_ON_DELAY    100     // delay between when we reach the minimum speed and we begin spraying.  This reduces the likelihood of constantly turning on/off the pump
+#define AC_SPRAYER_DEFAULT_SHUT_OFF_DELAY   1000    // shut-off delay in milli seconds.  This reduces the likelihood of constantly turning on/off the pump
+
+/// @class	Camera
+/// @brief	Object managing a Photo or video camera
+class AC_Sprayer {
+
+public:
+    /// Constructor
+    AC_Sprayer(AP_InertialNav* inav);
+
+    /// enable - allows sprayer to be enabled/disabled.  Note: this does not update the eeprom saved value
+    void enable(bool true_false);
+
+    /// enabled - returns true if fence is enabled
+    bool enabled() const { return _enabled; }
+
+    /// To-Do: add function to decode pilot input from channel 6 tuning knob
+
+    /// set_pump_rate - sets desired quantity of spray when travelling at 1m/s as a percentage of the pumps maximum rate
+    void set_pump_rate(float pct_at_1ms) { _pump_pct_1ms.set(pct_at_1ms); }
+
+    /// update - adjusts servo positions based on speed and requested quantity
+    void update();
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    // pointers to other objects we depend upon
+    AP_InertialNav* _inav;
+
+    // parameters
+    AP_Int8         _enabled;               // top level enable/disable control
+    AP_Float        _pump_pct_1ms;          // desired pump rate (expressed as a percentage of top rate) when travelling at 1m/s
+    AP_Int16        _spinner_pwm;           // pwm rate of spinner
+    AP_Float        _speed_min;             // minimum speed in cm/s above which the sprayer will be started
+
+    // internal variables
+    bool            _spraying;              // true if we are currently spraying
+    uint32_t        _speed_over_min_time;   // time at which we reached speed minimum
+    uint32_t        _speed_under_min_time;  // time at which we fell below speed minimum
+};
+
+#endif /* AC_SPRAYER_H */

libraries/AC_Sprayer/examples/AC_Sprayer_test/AC_Sprayer_test.pde

@@ -0,0 +1,83 @@
+/*
+ *       Example of AC_WPNav library .
+ *       DIYDrones.com
+ */
+
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Param.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+
+#include <AP_GPS.h>             // ArduPilot GPS library
+#include <AP_ADC.h>             // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_ADC_AnalogSource.h>
+#include <AP_Baro.h>            // ArduPilot Mega Barometer Library
+#include <Filter.h>
+#include <AP_Compass.h>         // ArduPilot Mega Magnetometer Library
+#include <AP_Declination.h>
+#include <AP_InertialSensor.h>  // ArduPilot Mega Inertial Sensor (accel & gyro) Library
+#include <AP_AHRS.h>
+#include <AP_Airspeed.h>
+#include <AC_PID.h>             // PID library
+#include <APM_PI.h>             // PID library
+#include <AP_Buffer.h>          // ArduPilot general purpose FIFO buffer
+#include <AP_InertialNav.h>     // Inertial Navigation library
+#include <GCS_MAVLink.h>
+#include <RC_Channel.h>
+#include <AC_Sprayer.h>         // Crop Sprayer library
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+// INS and Baro declaration
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
+
+#define A_LED_PIN 27
+#define C_LED_PIN 25
+AP_InertialSensor_MPU6000 ins;
+AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
+
+#else
+
+#define A_LED_PIN 37
+#define C_LED_PIN 35
+AP_ADC_ADS7844 adc;
+AP_InertialSensor_Oilpan ins(&adc);
+AP_Baro_BMP085 baro;
+#endif
+
+// GPS declaration
+GPS *gps;
+AP_GPS_Auto auto_gps(&gps);
+
+AP_Compass_HMC5843 compass;
+AP_AHRS_DCM ahrs(&ins, gps);
+
+// Inertial Nav declaration
+AP_InertialNav inertial_nav(&ahrs, &ins, &baro, &gps);
+
+// Sprayer
+AC_Sprayer sprayer(&inertial_nav);
+
+void setup()
+{
+    // display welcome message
+    hal.console->println("AC_Sprayer library test");
+
+    // enable sprayer
+    sprayer.enable(true);
+
+    // set-up pump rate
+    sprayer.set_pump_rate(1.0);
+}
+
+void loop()
+{
+    uint16_t i;
+
+    // repeated call sprayer at different velocities
+    sprayer.update();
+}
+
+AP_HAL_MAIN();

libraries/AC_Sprayer/examples/AC_Sprayer_test/Makefile

@@ -0,0 +1 @@
+include ../../../../mk/apm.mk

libraries/RC_Channel/RC_Channel_aux.h

@@ -48,6 +48,8 @@ public:
         k_elevator              = 19,            ///< elevator
         k_elevator_with_input   = 20,            ///< elevator, with rc input
         k_rudder                = 21,            ///< secondary rudder channel
+        k_sprayer_pump          = 22,            ///< crop sprayer pump channel
+        k_sprayer_spinner       = 23,            ///< crop sprayer spinner channel
         k_nr_aux_servo_functions         ///< This must be the last enum value (only add new values _before_ this one)
     } Aux_servo_function_t;