AP_ServoRelayEvents: library for handling servo and relay events

libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp

@@ -0,0 +1,129 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+ *   AP_ServoRelayEvents - handle servo and relay MAVLink events
+ */
+
+
+#include <AP_HAL.h>
+#include <AP_Common.h>
+#include <AP_ServoRelayEvents.h>
+#include <RC_Channel.h>
+
+extern const AP_HAL::HAL& hal;
+
+bool AP_ServoRelayEvents::do_set_servo(uint8_t channel, uint16_t pwm)
+{
+    if (!(mask & 1U<<(channel-1))) {
+        // not allowed
+        return false;
+    }
+    if (type == EVENT_TYPE_SERVO && 
+        channel == channel) {
+        // cancel previous repeat
+        repeat = 0;
+    }
+    hal.rcout->enable_ch(channel-1);
+    hal.rcout->write(channel-1, pwm);
+    return true;
+}
+
+bool AP_ServoRelayEvents::do_set_relay(uint8_t relay_num, uint8_t state)
+{
+    if (!relay.enabled(relay_num)) {
+        return false;
+    }
+    if (type == EVENT_TYPE_RELAY && 
+        channel == relay_num) {
+        // cancel previous repeat
+        repeat = 0;
+    }
+    if (state == 1) {
+        relay.on(relay_num);
+    } else if (state == 0) {
+        relay.off(relay_num);
+    } else {
+        relay.toggle(relay_num);
+    }
+    return true;
+}
+
+bool AP_ServoRelayEvents::do_repeat_servo(uint8_t _channel, uint16_t _servo_value, 
+                                          int16_t _repeat, uint16_t _delay_ms)
+{
+    if (!(mask & 1U<<(_channel-1))) {
+        // not allowed
+        return false;
+    }
+    channel = _channel;
+    type = EVENT_TYPE_SERVO;
+
+    start_time_ms  = 0;
+    delay_ms    = _delay_ms / 2;
+    repeat      = _repeat * 2;
+    servo_value = _servo_value;
+    update_events();
+    return true;
+}
+
+bool AP_ServoRelayEvents::do_repeat_relay(uint8_t relay_num, int16_t _repeat, uint32_t _delay_ms)
+{
+    if (!relay.enabled(relay_num)) {
+        return false;
+    }
+    type = EVENT_TYPE_RELAY;
+    channel = relay_num;
+    start_time_ms  = 0;
+    delay_ms        = _delay_ms/2; // half cycle time
+    repeat          = _repeat*2;  // number of full cycles
+    update_events();
+    return true;
+}
+
+
+/*
+  update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
+*/
+void AP_ServoRelayEvents::update_events(void)
+{
+    if (repeat == 0 || (hal.scheduler->millis() - start_time_ms) < delay_ms) {
+        return;
+    }
+
+    start_time_ms = hal.scheduler->millis();
+
+    switch (type) {
+    case EVENT_TYPE_SERVO:
+        hal.rcout->enable_ch(channel-1);
+        if (repeat & 1) {
+            hal.rcout->write(channel-1, RC_Channel::rc_channel(channel-1)->radio_trim);
+        } else {
+            hal.rcout->write(channel-1, servo_value);
+        }
+        break;
+        
+    case EVENT_TYPE_RELAY:
+        relay.toggle(channel);
+        break;
+    }
+    
+    if (repeat > 0) {
+        repeat--;
+    } else {
+        // toggle bottom bit so servos flip in value
+        repeat ^= 1;
+    }
+}

libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h

@@ -0,0 +1,60 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+ * AP_ServoRelayEvent.h
+ *
+ * handle DO_SET_SERVO, DO_REPEAT_SERVO, DO_SET_RELAY and
+ * DO_REPEAT_RELAY commands
+ */
+
+#ifndef __AP_SERVORELAYEVENTS_H__
+#define __AP_SERVORELAYEVENTS_H__
+
+#include <AP_Param.h>
+#include <AP_Relay.h>
+
+class AP_ServoRelayEvents {
+public:
+    AP_ServoRelayEvents(AP_Relay &_relay) : 
+    relay(_relay),
+    mask(0)
+        {}
+
+    // set allowed servo channel mask
+    void set_channel_mask(uint16_t _mask) { mask = _mask; }
+
+    bool do_set_servo(uint8_t channel, uint16_t pwm);
+    bool do_set_relay(uint8_t relay_num, uint8_t state);
+    bool do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repeat, uint16_t delay_time_ms);
+    bool do_repeat_relay(uint8_t relay_num, int16_t count, uint32_t period_ms);
+    void update_events(void);
+
+private:
+    AP_Relay &relay;
+    uint16_t mask;
+
+    // event control state
+    enum event_type { 
+        EVENT_TYPE_RELAY=0,
+        EVENT_TYPE_SERVO=1
+    };
+
+    enum event_type type;
+
+	// when the event was started in ms
+    uint32_t start_time_ms;
+
+	// how long to delay the next firing of event in millis
+    uint16_t delay_ms;
+
+	// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
+    int16_t repeat;
+
+    // RC channel for servos, relay number for relays
+    uint8_t channel;
+
+	// PWM for servos
+	uint16_t servo_value;
+};
+
+#endif /* AP_SERVORELAYEVENTS_H_ */