diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 62364fcd2d14c86e4c7feac2752cbc99c1bf7d95..714d352a8cc05682b3d60642cc3f9259d4019042 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -79,6 +79,7 @@
#include <ModeFilter.h> // Mode Filter from Filter library
#include <AverageFilter.h> // Mode Filter from Filter library
#include <AP_Relay.h> // APM relay
+#include <AP_ServoRelayEvents.h>
#include <AP_Mount.h> // Camera/Antenna mount
#include <AP_Camera.h> // Camera triggering
#include <GCS_MAVLink.h> // MAVLink GCS definitions
@@ -293,6 +294,8 @@ static AP_RangeFinder_analog sonar2;
// relay support
AP_Relay relay;
+AP_ServoRelayEvents ServoRelayEvents(relay);
+
// Camera
#if CAMERA == ENABLED
static AP_Camera camera(&relay);
@@ -467,22 +470,6 @@ static float wp_distance;
// Distance between previous and next waypoint. Meters
static int32_t wp_totalDistance;
-////////////////////////////////////////////////////////////////////////////////
-// repeating event control
-////////////////////////////////////////////////////////////////////////////////
-// Flag indicating current event type
-static uint8_t event_id;
-// when the event was started in ms
-static int32_t event_timer;
-// how long to delay the next firing of event in millis
-static uint16_t event_delay;
-// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
-static int16_t event_repeat = 0;
-// per command value, such as PWM for servos
-static int16_t event_value;
-// the value used to cycle events (alternate value to event_value)
-static int16_t event_undo_value;
-
////////////////////////////////////////////////////////////////////////////////
// Conditional command
////////////////////////////////////////////////////////////////////////////////
@@ -574,7 +561,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ read_trim_switch, 5, 1000 },
{ read_battery, 5, 1000 },
{ read_receiver_rssi, 5, 1000 },
- { update_events, 15, 1000 },
+ { update_events, 1, 1000 },
{ check_usb_mux, 15, 1000 },
{ mount_update, 1, 600 },
{ gcs_failsafe_check, 5, 600 },
diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index 5baf5a59ecc9b5108c4db2059fe23ee6e5b39645..b4f525da440083703bc482ce9cc102cc59176fcf 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -1142,9 +1142,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
- hal.rcout->enable_ch(packet.param1 - 1);
- hal.rcout->write(packet.param1 - 1, packet.param2);
- result = MAV_RESULT_ACCEPTED;
+ if (ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) {
+ result = MAV_RESULT_ACCEPTED;
+ }
+ break;
+
+ case MAV_CMD_DO_REPEAT_SERVO:
+ if (ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) {
+ result = MAV_RESULT_ACCEPTED;
+ }
+ break;
+
+ case MAV_CMD_DO_SET_RELAY:
+ if (ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) {
+ result = MAV_RESULT_ACCEPTED;
+ }
+ break;
+
+ case MAV_CMD_DO_REPEAT_RELAY:
+ if (ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {
+ result = MAV_RESULT_ACCEPTED;
+ }
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
@@ -1276,9 +1294,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
param1 = tell_command.p1;
break;
- case MAV_CMD_DO_REPEAT_SERVO:
- param4 = tell_command.lng;
- case MAV_CMD_DO_REPEAT_RELAY:
+ case MAV_CMD_DO_REPEAT_SERVO:
+ param4 = tell_command.lng*0.001f; // time
+ param3 = tell_command.lat; // repeat
+ param2 = tell_command.alt; // pwm
+ param1 = tell_command.p1; // channel
+ break;
+
+ case MAV_CMD_DO_REPEAT_RELAY:
+ param3 = tell_command.lat*0.001f; // time
+ param2 = tell_command.alt; // count
+ param1 = tell_command.p1; // relay number
+ break;
+
case MAV_CMD_DO_CHANGE_SPEED:
param3 = tell_command.lat;
param2 = tell_command.alt;
@@ -1567,8 +1595,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_REPEAT_SERVO:
- tell_command.lng = packet.param4;
+ tell_command.lng = packet.param4*1000; // time
+ tell_command.lat = packet.param3; // count
+ tell_command.alt = packet.param2; // PWM
+ tell_command.p1 = packet.param1; // channel
+ break;
+
case MAV_CMD_DO_REPEAT_RELAY:
+ tell_command.lat = packet.param3*1000; // time
+ tell_command.alt = packet.param2; // count
+ tell_command.p1 = packet.param1; // relay number
+ break;
+
case MAV_CMD_DO_CHANGE_SPEED:
tell_command.lat = packet.param3;
tell_command.alt = packet.param2;
diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde
index 7eae5e67a8499fc750ad0dd2e1c355bea259136c..d594249f4016d8c5fd24796e8ad2ce163df33d7a 100644
--- a/APMrover2/commands_logic.pde
+++ b/APMrover2/commands_logic.pde
@@ -66,21 +66,23 @@ static void handle_process_do_command()
do_set_home();
break;
- case MAV_CMD_DO_SET_SERVO:
- do_set_servo();
- break;
+ case MAV_CMD_DO_SET_SERVO:
+ ServoRelayEvents.do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
+ break;
- case MAV_CMD_DO_SET_RELAY:
- do_set_relay();
- break;
+ case MAV_CMD_DO_SET_RELAY:
+ ServoRelayEvents.do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
+ break;
- case MAV_CMD_DO_REPEAT_SERVO:
- do_repeat_servo();
- break;
+ case MAV_CMD_DO_REPEAT_SERVO:
+ ServoRelayEvents.do_repeat_servo(next_nonnav_command.p1, next_nonnav_command.alt,
+ next_nonnav_command.lat, next_nonnav_command.lng);
+ break;
- case MAV_CMD_DO_REPEAT_RELAY:
- do_repeat_relay();
- break;
+ case MAV_CMD_DO_REPEAT_RELAY:
+ ServoRelayEvents.do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt,
+ next_nonnav_command.lat);
+ break;
#if CAMERA == ENABLED
case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
@@ -366,47 +368,6 @@ static void do_set_home()
}
}
-static void do_set_servo()
-{
- hal.rcout->enable_ch(next_nonnav_command.p1 - 1);
- hal.rcout->write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
-}
-
-static void do_set_relay()
-{
- if (next_nonnav_command.p1 == 1) {
- relay.on(0);
- } else if (next_nonnav_command.p1 == 0) {
- relay.off(0);
- }else{
- relay.toggle(0);
- }
-}
-
-static void do_repeat_servo()
-{
- event_id = next_nonnav_command.p1 - 1;
-
- if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
- event_timer = 0;
- event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
- event_repeat = next_nonnav_command.lat * 2;
- event_value = next_nonnav_command.alt;
- event_undo_value = RC_Channel::rc_channel(next_nonnav_command.p1-1)->radio_trim;
- update_events();
- }
-}
-
-static void do_repeat_relay()
-{
- event_id = RELAY_TOGGLE;
- event_timer = 0;
- event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
- event_repeat = next_nonnav_command.alt * 2;
- update_events();
-}
-
-
// do_take_picture - take a picture with the camera library
static void do_take_picture()
{
diff --git a/APMrover2/events.pde b/APMrover2/events.pde
index da008c2cb71cd7dadac74eca4b39b4a03c4223e0..f4a07313df0d4496747d987ef75593cdcd581a71 100644
--- a/APMrover2/events.pde
+++ b/APMrover2/events.pde
@@ -1,28 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
+static void update_events(void)
{
- if(event_repeat == 0 || (millis() - event_timer) < event_delay)
- return;
-
- if (event_repeat > 0){
- event_repeat --;
- }
-
- if(event_repeat != 0) { // event_repeat = -1 means repeat forever
- event_timer = millis();
-
- if (event_id >= CH_5 && event_id <= CH_8) {
- if(event_repeat%2) {
- hal.rcout->write(event_id, event_value); // send to Servos
- } else {
- hal.rcout->write(event_id, event_undo_value);
- }
- }
-
- if (event_id == RELAY_TOGGLE) {
- relay.toggle(0);
- }
- }
+ ServoRelayEvents.update_events();
}
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index d1710a33676f68f0bf39a6ea1ddc67ef92f1be3b..888e591cf64cdb9732e4ebd580a9791983f63210 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -106,6 +106,8 @@ static void init_ardupilot()
BoardConfig.init();
+ ServoRelayEvents.set_channel_mask(0xFFF0);
+
set_control_channels();
// after parameter load setup correct baud rate on uartA