From cdd36b2c419976d3e102fb73cb3baf86caeee14f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Jan 2014 15:36:31 +1100 Subject: [PATCH] Plane: use AP_ServoRelayEvents library --- ArduPlane/ArduPlane.pde | 33 +++--------------- ArduPlane/GCS_Mavlink.pde | 20 ++++++----- ArduPlane/commands_logic.pde | 67 ++++-------------------------------- ArduPlane/events.pde | 37 +------------------- ArduPlane/system.pde | 3 ++ 5 files changed, 26 insertions(+), 134 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 28e8b4fb92..11d017ffdf 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -75,6 +75,7 @@ #include #include +#include // 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 9e6d7366f6..81afc20656 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 a6fa237860..4c09a9a4e1 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 ed03f7468c..2027fa9433 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 92b44f3f8a..10a6ff5a80 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