diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 1a8bf3d7fd9facdbff79835ff13c84403d14c375..fddda50d4f3e01271b99299ce598abaddb38ee7f 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -115,6 +115,7 @@
#include <Filter.h> // Filter library
#include <AP_Buffer.h> // APM FIFO Buffer
#include <AP_Relay.h> // APM relay
+#include <AP_ServoRelayEvents.h>
#include <AP_Camera.h> // Photo or video camera
#include <AP_Mount.h> // Camera/Antenna mount
#include <AP_Airspeed.h> // needed for AHRS build
@@ -725,22 +726,6 @@ static int16_t yaw_look_at_heading_slew;
-////////////////////////////////////////////////////////////////////////////////
-// Repeat Mission Scripting Command
-////////////////////////////////////////////////////////////////////////////////
-// The type of repeating event - Toggle a servo channel, Toggle the APM1 relay, etc
-static uint8_t event_id;
-// Used to manage the timimng of repeating events
-static uint32_t event_timer;
-// How long to delay the next firing of event in millis
-static uint16_t event_delay;
-// how many times to fire : 0 = forever, 1 = do once, 2 = do twice
-static int16_t event_repeat;
-// per command value, such as PWM for servos
-static int16_t event_value;
-// the stored value used to undo commands - such as original PWM command
-static int16_t event_undo_value;
-
////////////////////////////////////////////////////////////////////////////////
// Delay Mission Scripting Command
////////////////////////////////////////////////////////////////////////////////
@@ -786,6 +771,9 @@ static uint8_t auto_trim_counter;
// Reference to the relay object (APM1 -> PORTL 2) (APM2 -> PORTB 7)
static AP_Relay relay;
+// handle repeated servo and relay events
+static AP_ServoRelayEvents ServoRelayEvents(relay);
+
//Reference to the camera object (it uses the relay object inside it)
#if CAMERA == ENABLED
static AP_Camera camera(&relay);
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index e843dd9ed2a4cf9c6a2b31e921a16381d9606618..a6869bc311f3590922e365da5edc3447ee2ac584 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -1216,8 +1216,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
- servo_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:
@@ -1374,8 +1393,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;
@@ -1666,8 +1695,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/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index b16680b547bde6a20af8a2c3f7b0f5d56b296625..75828452ad80461b321471efad0865259152a993 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -86,20 +86,22 @@ static void process_now_command()
do_set_home();
break;
- case MAV_CMD_DO_SET_SERVO: // 183
- do_set_servo();
+ case MAV_CMD_DO_SET_SERVO:
+ ServoRelayEvents.do_set_servo(command_cond_queue.p1, command_cond_queue.alt);
break;
-
- case MAV_CMD_DO_SET_RELAY: // 181
- do_set_relay();
+
+ case MAV_CMD_DO_SET_RELAY:
+ ServoRelayEvents.do_set_relay(command_cond_queue.p1, command_cond_queue.alt);
break;
-
- case MAV_CMD_DO_REPEAT_SERVO: // 184
- do_repeat_servo();
+
+ case MAV_CMD_DO_REPEAT_SERVO:
+ ServoRelayEvents.do_repeat_servo(command_cond_queue.p1, command_cond_queue.alt,
+ command_cond_queue.lat, command_cond_queue.lng);
break;
-
- case MAV_CMD_DO_REPEAT_RELAY: // 182
- do_repeat_relay();
+
+ case MAV_CMD_DO_REPEAT_RELAY:
+ ServoRelayEvents.do_repeat_relay(command_cond_queue.p1, command_cond_queue.alt,
+ command_cond_queue.lat);
break;
case MAV_CMD_DO_SET_ROI: // 201
@@ -898,60 +900,6 @@ static void do_set_home()
}
}
-static void do_set_servo()
-{
- servo_write(command_cond_queue.p1 - 1, command_cond_queue.alt);
-}
-
-static void do_set_relay()
-{
- if (command_cond_queue.p1 == 1) {
- relay.on(0);
- } else if (command_cond_queue.p1 == 0) {
- relay.off(0);
- }else{
- relay.toggle(0);
- }
-}
-
-static void do_repeat_servo()
-{
- event_id = command_cond_queue.p1 - 1;
-
- if(command_cond_queue.p1 >= CH_5 + 1 && command_cond_queue.p1 <= CH_8 + 1) {
-
- event_timer = 0;
- event_value = command_cond_queue.alt;
- event_repeat = command_cond_queue.lat * 2;
- event_delay = command_cond_queue.lng * 500.0f; // /2 (half cycle time) * 1000 (convert to milliseconds)
-
- switch(command_cond_queue.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();
- }
-}
-
-static void do_repeat_relay()
-{
- event_id = RELAY_TOGGLE;
- event_timer = 0;
- event_delay = command_cond_queue.lat * 500.0f; // /2 (half cycle time) * 1000 (convert to milliseconds)
- event_repeat = command_cond_queue.alt * 2;
- update_events();
-}
-
// do_roi - starts actions required by MAV_CMD_NAV_ROI
// this involves either moving the camera to point at the ROI (region of interest)
// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde
index ce66ffe8845c13c8588ebf52cde02d0ed78bd038..7782f057e3999ed682b672c935077fab238bc02c 100644
--- a/ArduCopter/events.pde
+++ b/ArduCopter/events.pde
@@ -307,28 +307,8 @@ static void failsafe_gcs_off_event(void)
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
}
-static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
+static void update_events()
{
- if(event_repeat == 0 || (millis() - event_timer) < event_delay)
- return;
-
- 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);
- }
- if (event_repeat > 0) {
- event_repeat--;
- }
- }
+ ServoRelayEvents.update_events();
}
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 9a1685c7ce5c0013003cfd4001bc7def92d78831..1bdf46a866506f584a8b72e1c5308444e8081cc7 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -125,6 +125,9 @@ static void init_ardupilot()
BoardConfig.init();
+ // FIX: this needs to be the inverse motors mask
+ ServoRelayEvents.set_channel_mask(0xFFF0);
+
relay.init();
bool enable_external_leds = true;