diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index cc621efbc9d41964f63d42c34a38894e352588fb..0c3bedff5efdfe645bd841edaa8d3a9bd6430c93 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -617,8 +617,8 @@ static struct {
// 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;
+ // RC channel for servos, relay number for relays
+ uint8_t channel;
// PWM for servos
uint16_t servo_value;
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index acf62373ad896ba7455d1d157cd229f7ab001542..8c646f58fbbe7f6ce05bd00cecccafe4ff00e4e3 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -1309,7 +1309,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_REPEAT_SERVO:
- do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4);
+ 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;
+ break;
+
+ case MAV_CMD_DO_REPEAT_RELAY:
+ do_repeat_relay(packet.param1, packet.param2, packet.param3*1000);
result = MAV_RESULT_ACCEPTED;
break;
@@ -1462,8 +1472,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;
@@ -1763,10 +1783,20 @@ 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.lat = packet.param3*1000; // convert to milliseconds
tell_command.alt = packet.param2;
tell_command.p1 = packet.param1;
break;
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index e3ad808116a6ec0c3388961c2a9052d6b943acbc..e5715a0b4218f45f8233e3e1433b1e1032464eba 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -94,7 +94,7 @@ static void handle_process_do_command()
break;
case MAV_CMD_DO_SET_RELAY:
- do_set_relay();
+ do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
break;
case MAV_CMD_DO_REPEAT_SERVO:
@@ -103,7 +103,8 @@ static void handle_process_do_command()
break;
case MAV_CMD_DO_REPEAT_RELAY:
- do_repeat_relay();
+ do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt,
+ next_nonnav_command.lat);
break;
#if CAMERA == ENABLED
@@ -620,46 +621,45 @@ static void do_set_servo()
servo_write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
}
-static void do_set_relay()
+static void do_set_relay(uint8_t relay_num, uint8_t state)
{
- if (next_nonnav_command.p1 == 1) {
+ if (state == 1) {
gcs_send_text_fmt(PSTR("Relay on"));
- relay.on();
- } else if (next_nonnav_command.p1 == 0) {
+ relay.on(relay_num);
+ } else if (state == 0) {
gcs_send_text_fmt(PSTR("Relay off"));
- relay.off();
- }else{
+ relay.off(relay_num);
+ } else {
gcs_send_text_fmt(PSTR("Relay toggle"));
- relay.toggle();
+ relay.toggle(relay_num);
}
}
-static void do_repeat_servo(uint8_t channel, uint16_t servo_value,
- int16_t repeat, uint8_t delay_time)
+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 > 8) {
+ if (channel < 5 || channel > 16) {
// not allowed
return;
}
- event_state.rc_channel = channel;
+ event_state.channel = channel;
event_state.type = EVENT_TYPE_SERVO;
event_state.start_time_ms = 0;
- event_state.delay_ms = delay_time * 500UL;
+ 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;
update_events();
}
-static void do_repeat_relay()
+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;
- // /2 (half cycle time) * 1000 (convert to milliseconds)
- event_state.delay_ms = next_nonnav_command.lat * 500.0;
- event_state.repeat = next_nonnav_command.alt * 2;
+ event_state.delay_ms = period_ms/2; // half cycle time
+ event_state.repeat = count*2; // number of full cycles
update_events();
}
diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde
index bd1b03b2f95ce60189a1716647a821e72b56a49c..8b766a02a86b1b655f6586d6c4f742c563dc4b00 100644
--- a/ArduPlane/events.pde
+++ b/ArduPlane/events.pde
@@ -133,17 +133,17 @@ static void update_events(void)
switch (event_state.type) {
case EVENT_TYPE_SERVO:
- hal.rcout->enable_ch(event_state.rc_channel);
+ hal.rcout->enable_ch(event_state.channel);
if (event_state.repeat & 1) {
- servo_write(event_state.rc_channel, event_state.undo_value);
+ servo_write(event_state.channel, event_state.undo_value);
} else {
- servo_write(event_state.rc_channel, event_state.servo_value);
+ servo_write(event_state.channel, event_state.servo_value);
}
break;
case EVENT_TYPE_RELAY:
gcs_send_text_fmt(PSTR("Relay toggle"));
- relay.toggle();
+ relay.toggle(event_state.channel);
break;
}
diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde
index 0c3a36411138f5d8f20dd5632bd9badebbe14f98..bbbc0c1cf033ad57972714753fc3fe2a31f11978 100644
--- a/ArduPlane/test.pde
+++ b/ArduPlane/test.pde
@@ -255,14 +255,14 @@ test_relay(uint8_t argc, const Menu::arg *argv)
while(1) {
cliSerial->printf_P(PSTR("Relay on\n"));
- relay.on();
+ relay.on(0);
delay(3000);
if(cliSerial->available() > 0) {
return (0);
}
cliSerial->printf_P(PSTR("Relay off\n"));
- relay.off();
+ relay.off(0);
delay(3000);
if(cliSerial->available() > 0) {
return (0);