APM: fixed handling of DO_REPEAT_SERVO and DO_REPEAT_RELAY

ArduPlane/ArduPlane.pde

@@ -565,24 +565,34 @@ int32_t wp_distance;
 // Distance between previous and next waypoint.  Meters
 static int32_t wp_totalDistance;
 
-////////////////////////////////////////////////////////////////////////////////
+// event control state
-// repeating event control
+enum event_type { 
-////////////////////////////////////////////////////////////////////////////////
+    EVENT_TYPE_RELAY=0,
-// Flag indicating current event type
+    EVENT_TYPE_SERVO=1
-static byte event_id;
+};
 
-// when the event was started in ms
+static struct {
-static int32_t event_timer_ms;
+    enum event_type type;
 
-// how long to delay the next firing of event in millis
+	// when the event was started in ms
-static uint16_t event_delay_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
+    uint8_t rc_channel;
+
+	// PWM for servos
+	uint16_t servo_value;
+
+	// the value used to cycle events (alternate value to event_value)
+    uint16_t undo_value;
+} event_state;
 
-// 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

ArduPlane/GCS_Mavlink.pde

@@ -1111,6 +1111,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             result = MAV_RESULT_ACCEPTED;
             break;
 
+        case MAV_CMD_DO_REPEAT_SERVO:
+            do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4);
+            result = MAV_RESULT_ACCEPTED;
+            break;
+
         case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
             if (packet.param1 == 1) {
 #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2

ArduPlane/commands_logic.pde

@@ -98,7 +98,8 @@ static void handle_process_do_command()
         break;
 
     case MAV_CMD_DO_REPEAT_SERVO:
-        do_repeat_servo();
+        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:
@@ -591,40 +592,32 @@ static void do_set_relay()
     }
 }
 
-static void do_repeat_servo()
+static void do_repeat_servo(uint8_t channel, uint16_t servo_value,
+                            int16_t repeat, uint8_t delay_time)
 {
-    event_id = next_nonnav_command.p1 - 1;
+    extern RC_Channel *rc_ch[NUM_CHANNELS];
-
+    channel = channel - 1;
-    if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
+    if (channel < 5 || channel > 8) {
-
+        // not allowed
-        event_timer_ms  = 0;
+        return;
-        event_delay_ms  = 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;
-
-        switch(next_nonnav_command.p1) {
-        case CH_5:
-            event_undo_value = g.rc_5.radio_trim;
-            break;
-        case CH_6:
-            event_undo_value = g.rc_6.radio_trim;
-            break;
-        case CH_7:
-            event_undo_value = g.rc_7.radio_trim;
-            break;
-        case CH_8:
-            event_undo_value = g.rc_8.radio_trim;
-            break;
-        }
-        update_events();
     }
+    event_state.rc_channel = channel;
+    event_state.type = EVENT_TYPE_SERVO;
+
+    event_state.start_time_ms  = 0;
+    event_state.delay_ms    = delay_time * 500UL;
+    event_state.repeat      = repeat * 2;
+    event_state.servo_value = servo_value;
+    event_state.undo_value  = rc_ch[channel]->radio_trim;
+    update_events();
 }
 
 static void do_repeat_relay()
 {
-    event_id                = RELAY_TOGGLE;
+    event_state.type = EVENT_TYPE_RELAY;
-    event_timer_ms  = 0;
+    event_state.start_time_ms  = 0;
-    event_delay_ms  = next_nonnav_command.lat * 500.0;          // /2 (half cycle time) * 1000 (convert to milliseconds)
+    // /2 (half cycle time) * 1000 (convert to milliseconds)
-    event_repeat    = next_nonnav_command.alt * 2;
+    event_state.delay_ms        = next_nonnav_command.lat * 500.0;
+    event_state.repeat          = next_nonnav_command.alt * 2;
     update_events();
 }

ArduPlane/events.pde

@@ -86,28 +86,39 @@ void low_battery_event(void)
 }
 #endif
 
-static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
+////////////////////////////////////////////////////////////////////////////////
-{
+// repeating event control
-    if(event_repeat == 0 || (millis() - event_timer_ms) < event_delay_ms)
-        return;
 
-    if (event_repeat > 0) {
+/*
-        event_repeat--;
+  update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
+*/
+static void update_events(void)
+{
+    if (event_state.repeat == 0 || (millis() - event_state.start_time_ms) < event_state.delay_ms) {
+        return;
     }
 
-    if(event_repeat != 0) {             // event_repeat = -1 means repeat forever
+    // event_repeat = -1 means repeat forever
-        event_timer_ms = millis();
+    if (event_state.repeat != 0) {
+        event_state.start_time_ms = millis();
 
-        if (event_id >= CH_5 && event_id <= CH_8) {
+        switch (event_state.type) {
-            if(event_repeat%2) {
+        case EVENT_TYPE_SERVO:
-                APM_RC.OutputCh(event_id, event_value);                 // send to Servos
+            APM_RC.enable_out(event_state.rc_channel);
+            if (event_state.repeat & 1) {
+                APM_RC.OutputCh(event_state.rc_channel, event_state.undo_value);                 
             } else {
-                APM_RC.OutputCh(event_id, event_undo_value);
+                APM_RC.OutputCh(event_state.rc_channel, event_state.servo_value);                 
             }
+            break;
+
+        case EVENT_TYPE_RELAY:
+            relay.toggle();
+            break;
         }
 
-        if  (event_id == RELAY_TOGGLE) {
+        if (event_state.repeat > 0) {
-            relay.toggle();
+            event_state.repeat--;
         }
     }
 }