diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 0c3bedff5efdfe645bd841edaa8d3a9bd6430c93..28e8b4fb92bfde1df3e2f4d31ecb7a07e578edac 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -747,7 +747,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { obc_fs_check, 5, 1000 }, { gcs_update, 1, 1700 }, { gcs_data_stream_send, 1, 3000 }, - { update_events, 15, 1500 }, // 20 + { update_events, 1, 1500 }, // 20 { check_usb_mux, 5, 300 }, { read_battery, 5, 1000 }, { compass_accumulate, 1, 1500 }, diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 8c646f58fbbe7f6ce05bd00cecccafe4ff00e4e3..9e6d7366f6f28e58ca946cb2418d4a1b9e34eaf1 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1304,7 +1304,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_SET_SERVO: - servo_write(packet.param1 - 1, packet.param2); + do_set_servo(packet.param1, packet.param2); result = MAV_RESULT_ACCEPTED; break; diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index e5715a0b4218f45f8233e3e1433b1e1032464eba..a6fa2378609544b5f6fa2efef651eca2eb39c0e5 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -90,7 +90,7 @@ static void handle_process_do_command() break; case MAV_CMD_DO_SET_SERVO: - do_set_servo(); + do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt); break; case MAV_CMD_DO_SET_RELAY: @@ -616,13 +616,22 @@ static void do_set_home() } } -static void do_set_servo() +static void do_set_servo(uint8_t channel, uint16_t pwm) { - servo_write(next_nonnav_command.p1 - 1, next_nonnav_command.alt); + 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); @@ -637,7 +646,6 @@ static void do_set_relay(uint8_t relay_num, uint8_t state) 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 > 16) { // not allowed return; @@ -649,7 +657,7 @@ static void do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repea 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; + event_state.undo_value = RC_Channel::rc_channel(channel-1)->radio_trim; update_events(); } diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index 8b766a02a86b1b655f6586d6c4f742c563dc4b00..ed03f7468c05a99534eaa3e221a5f4fbc671d2d3 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -127,28 +127,30 @@ static void update_events(void) return; } - // event_repeat = -1 means repeat forever - if (event_state.repeat != 0) { - event_state.start_time_ms = millis(); + event_state.start_time_ms = millis(); - switch (event_state.type) { - case EVENT_TYPE_SERVO: - hal.rcout->enable_ch(event_state.channel); - if (event_state.repeat & 1) { - servo_write(event_state.channel, event_state.undo_value); - } else { - servo_write(event_state.channel, event_state.servo_value); - } - break; - - case EVENT_TYPE_RELAY: - gcs_send_text_fmt(PSTR("Relay toggle")); - relay.toggle(event_state.channel); - break; + 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); } - - if (event_state.repeat > 0) { - event_state.repeat--; + 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; } }