Rover: converted to AP_ServoRelayEvents

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 },

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);
+            if (ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) {
-            hal.rcout->write(packet.param1 - 1, 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:
@@ -1277,8 +1295,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					break;
 
             	case MAV_CMD_DO_REPEAT_SERVO:
-					param4 = tell_command.lng;
+                    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;

APMrover2/commands_logic.pde

@@ -67,19 +67,21 @@ static void handle_process_do_command()
 			break;
 
     	case MAV_CMD_DO_SET_SERVO:
-			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();
+            ServoRelayEvents.do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
             break;
 
     	case MAV_CMD_DO_REPEAT_SERVO:
-			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();
+            ServoRelayEvents.do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt,
+                                             next_nonnav_command.lat);
             break;
 
 #if CAMERA == ENABLED
@@ -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()
 {

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)
+    ServoRelayEvents.update_events();
-		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);
-		}
-	}
 }

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