From 2045591bed0f2470582b24cc5719576de760fd8f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 20 Jan 2014 14:52:05 +1100
Subject: [PATCH] Plane: fixed servo and relay repeat code

---
 ArduPlane/ArduPlane.pde      |  2 +-
 ArduPlane/GCS_Mavlink.pde    |  2 +-
 ArduPlane/commands_logic.pde | 18 +++++++++++-----
 ArduPlane/events.pde         | 42 +++++++++++++++++++-----------------
 4 files changed, 37 insertions(+), 27 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 0c3bedff5e..28e8b4fb92 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -747,7 +747,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { obc_fs_check,           5,   1000 },
     { gcs_update,             1,   1700 },
     { gcs_data_stream_send,   1,   3000 },
-    { update_events,		 15,   1500 }, // 20
+    { update_events,		  1,   1500 }, // 20
     { check_usb_mux,          5,    300 },
     { read_battery,           5,   1000 },
     { compass_accumulate,     1,   1500 },
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 8c646f58fb..9e6d7366f6 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -1304,7 +1304,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_DO_SET_SERVO:
-            servo_write(packet.param1 - 1, packet.param2);
+            do_set_servo(packet.param1, packet.param2);
             result = MAV_RESULT_ACCEPTED;
             break;
 
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index e5715a0b42..a6fa237860 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -90,7 +90,7 @@ static void handle_process_do_command()
         break;
 
     case MAV_CMD_DO_SET_SERVO:
-        do_set_servo();
+        do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
         break;
 
     case MAV_CMD_DO_SET_RELAY:
@@ -616,13 +616,22 @@ static void do_set_home()
     }
 }
 
-static void do_set_servo()
+static void do_set_servo(uint8_t channel, uint16_t pwm)
 {
-    servo_write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
+    if (event_state.type == EVENT_TYPE_SERVO && 
+        event_state.channel == channel) {
+        event_state.repeat = 0;
+    }
+    servo_write(channel-1, pwm);
 }
 
 static void do_set_relay(uint8_t relay_num, uint8_t state)
 {
+    if (event_state.type == EVENT_TYPE_RELAY && 
+        event_state.channel == relay_num) {
+        event_state.repeat = 0;
+    }
+
     if (state == 1) {
         gcs_send_text_fmt(PSTR("Relay on"));
         relay.on(relay_num);
@@ -637,7 +646,6 @@ static void do_set_relay(uint8_t relay_num, uint8_t state)
 
 static void do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repeat, uint16_t delay_time_ms)
 {
-    channel = channel - 1;
     if (channel < 5 || channel > 16) {
         // not allowed
         return;
@@ -649,7 +657,7 @@ static void do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repea
     event_state.delay_ms    = delay_time_ms / 2;
     event_state.repeat      = repeat * 2;
     event_state.servo_value = servo_value;
-    event_state.undo_value  = RC_Channel::rc_channel(channel)->radio_trim;
+    event_state.undo_value  = RC_Channel::rc_channel(channel-1)->radio_trim;
     update_events();
 }
 
diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde
index 8b766a02a8..ed03f7468c 100644
--- a/ArduPlane/events.pde
+++ b/ArduPlane/events.pde
@@ -127,28 +127,30 @@ static void update_events(void)
         return;
     }
 
-    // event_repeat = -1 means repeat forever
-    if (event_state.repeat != 0) {
-        event_state.start_time_ms = millis();
+    event_state.start_time_ms = millis();
 
-        switch (event_state.type) {
-        case EVENT_TYPE_SERVO:
-            hal.rcout->enable_ch(event_state.channel);
-            if (event_state.repeat & 1) {
-                servo_write(event_state.channel, event_state.undo_value);
-            } else {
-                servo_write(event_state.channel, event_state.servo_value);                 
-            }
-            break;
-
-        case EVENT_TYPE_RELAY:
+    switch (event_state.type) {
+    case EVENT_TYPE_SERVO:
+        hal.rcout->enable_ch(event_state.channel-1);
+        if (event_state.repeat & 1) {
+            servo_write(event_state.channel-1, event_state.undo_value);
+        } else {
+            servo_write(event_state.channel-1, event_state.servo_value);                 
+        }
+        break;
+        
+    case EVENT_TYPE_RELAY:
+        if (event_state.delay_ms >= 1000) {
+            // don't spam the GCS with messages too fast
             gcs_send_text_fmt(PSTR("Relay toggle"));
-            relay.toggle(event_state.channel);
-            break;
-        }
-
-        if (event_state.repeat > 0) {
-            event_state.repeat--;
         }
+        relay.toggle(event_state.channel);
+        break;
+    }
+    
+    if (event_state.repeat > 0) {
+        event_state.repeat--;
+    } else {
+        event_state.repeat ^= 1;
     }
 }