diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 28e8b4fb92bfde1df3e2f4d31ecb7a07e578edac..11d017ffdf9b198db21729b22cf8075cd19a5ba7 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -75,6 +75,7 @@
#include <AP_Arming.h>
#include <AP_BoardConfig.h>
+#include <AP_ServoRelayEvents.h>
// Pre-AP_HAL compatibility
#include "compat.h"
@@ -325,6 +326,9 @@ static AP_RangeFinder_analog sonar;
////////////////////////////////////////////////////////////////////////////////
static AP_Relay relay;
+// handle servo and relay events
+static AP_ServoRelayEvents ServoRelayEvents(relay);
+
// Camera
#if CAMERA == ENABLED
static AP_Camera camera(&relay);
@@ -599,35 +603,6 @@ static struct {
} loiter;
-// event control state
-enum event_type {
- EVENT_TYPE_RELAY=0,
- EVENT_TYPE_SERVO=1
-};
-
-static struct {
- enum event_type type;
-
- // when the event was started in 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, relay number for relays
- uint8_t 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;
-
-
////////////////////////////////////////////////////////////////////////////////
// Conditional command
////////////////////////////////////////////////////////////////////////////////
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 9e6d7366f6f28e58ca946cb2418d4a1b9e34eaf1..81afc20656e7a7496d2a8a74c6c269833902a717 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -1304,23 +1304,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
- do_set_servo(packet.param1, 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:
- do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000);
- result = MAV_RESULT_ACCEPTED;
+ 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:
- do_set_relay(packet.param1, packet.param2);
- result = MAV_RESULT_ACCEPTED;
+ if (ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) {
+ result = MAV_RESULT_ACCEPTED;
+ }
break;
case MAV_CMD_DO_REPEAT_RELAY:
- do_repeat_relay(packet.param1, packet.param2, packet.param3*1000);
- result = MAV_RESULT_ACCEPTED;
+ if (ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {
+ result = MAV_RESULT_ACCEPTED;
+ }
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index a6fa2378609544b5f6fa2efef651eca2eb39c0e5..4c09a9a4e11598e7e90f0da016460be1d3024b19 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -90,21 +90,21 @@ static void handle_process_do_command()
break;
case MAV_CMD_DO_SET_SERVO:
- do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
+ ServoRelayEvents.do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
break;
case MAV_CMD_DO_SET_RELAY:
- do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
+ ServoRelayEvents.do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
break;
case MAV_CMD_DO_REPEAT_SERVO:
- do_repeat_servo(next_nonnav_command.p1, next_nonnav_command.alt,
- next_nonnav_command.lat, next_nonnav_command.lng);
+ 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(next_nonnav_command.p1, next_nonnav_command.alt,
- next_nonnav_command.lat);
+ ServoRelayEvents.do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt,
+ next_nonnav_command.lat);
break;
#if CAMERA == ENABLED
@@ -616,61 +616,6 @@ static void do_set_home()
}
}
-static void do_set_servo(uint8_t channel, uint16_t pwm)
-{
- 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);
- } else if (state == 0) {
- gcs_send_text_fmt(PSTR("Relay off"));
- relay.off(relay_num);
- } else {
- gcs_send_text_fmt(PSTR("Relay toggle"));
- relay.toggle(relay_num);
- }
-}
-
-static void do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repeat, uint16_t delay_time_ms)
-{
- if (channel < 5 || channel > 16) {
- // not allowed
- return;
- }
- event_state.channel = channel;
- event_state.type = EVENT_TYPE_SERVO;
-
- event_state.start_time_ms = 0;
- 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-1)->radio_trim;
- update_events();
-}
-
-static void do_repeat_relay(uint8_t relay_num, int16_t count, uint32_t period_ms)
-{
- event_state.type = EVENT_TYPE_RELAY;
- event_state.channel = relay_num;
- event_state.start_time_ms = 0;
- event_state.delay_ms = period_ms/2; // half cycle time
- event_state.repeat = count*2; // number of full cycles
- update_events();
-}
-
// do_take_picture - take a picture with the camera library
static void do_take_picture()
{
diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde
index ed03f7468c05a99534eaa3e221a5f4fbc671d2d3..2027fa943320dacda31e3e776db1e12e71242aea 100644
--- a/ArduPlane/events.pde
+++ b/ArduPlane/events.pde
@@ -115,42 +115,7 @@ void low_battery_event(void)
AP_Notify::flags.failsafe_battery = true;
}
-////////////////////////////////////////////////////////////////////////////////
-// repeating event control
-
-/*
- 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;
- }
-
- event_state.start_time_ms = millis();
-
- 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--;
- } else {
- event_state.repeat ^= 1;
- }
+ ServoRelayEvents.update_events();
}
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 92b44f3f8a067904a482d4dfb2d1895dbf1a8b7d..10a6ff5a80d67c76cd45943f9491bb0e07bbfdfd 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -94,6 +94,9 @@ static void init_ardupilot()
BoardConfig.init();
+ // allow servo set on all channels except first 4
+ ServoRelayEvents.set_channel_mask(0xFFF0);
+
set_control_channels();
// reset the uartA baud rate after parameter load