From 7e8ef0ae95b6d4464f1c45ed3b34d9b4bb26ca45 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Sep 2012 09:24:00 +1000 Subject: [PATCH] APM: fixed handling of DO_REPEAT_SERVO and DO_REPEAT_RELAY --- ArduPlane/ArduPlane.pde | 40 +++++++++++++++++---------- ArduPlane/GCS_Mavlink.pde | 5 ++++ ArduPlane/commands_logic.pde | 53 ++++++++++++++++-------------------- ArduPlane/events.pde | 39 ++++++++++++++++---------- 4 files changed, 78 insertions(+), 59 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 04d3fab569..7f187264bf 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -565,24 +565,34 @@ int32_t wp_distance; // Distance between previous and next waypoint. Meters static int32_t wp_totalDistance; -//////////////////////////////////////////////////////////////////////////////// -// repeating event control -//////////////////////////////////////////////////////////////////////////////// -// Flag indicating current event type -static byte event_id; +// event control state +enum event_type { + EVENT_TYPE_RELAY=0, + EVENT_TYPE_SERVO=1 +}; -// when the event was started in ms -static int32_t event_timer_ms; +static struct { + enum event_type type; -// how long to delay the next firing of event in millis -static uint16_t event_delay_ms; + // 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 + uint8_t rc_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; -// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles -static int16_t event_repeat = 0; -// per command value, such as PWM for servos -static int16_t event_value; -// the value used to cycle events (alternate value to event_value) -static int16_t event_undo_value; //////////////////////////////////////////////////////////////////////////////// // Conditional command diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index eb4514933c..374aa039e8 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1111,6 +1111,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_ACCEPTED; break; + case MAV_CMD_DO_REPEAT_SERVO: + do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4); + result = MAV_RESULT_ACCEPTED; + break; + case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (packet.param1 == 1) { #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 9786b0e3b1..b6f46ec32e 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -98,7 +98,8 @@ static void handle_process_do_command() break; case MAV_CMD_DO_REPEAT_SERVO: - do_repeat_servo(); + 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: @@ -591,40 +592,32 @@ static void do_set_relay() } } -static void do_repeat_servo() +static void do_repeat_servo(uint8_t channel, uint16_t servo_value, + int16_t repeat, uint8_t delay_time) { - event_id = next_nonnav_command.p1 - 1; - - if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) { - - event_timer_ms = 0; - event_delay_ms = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_nonnav_command.lat * 2; - event_value = next_nonnav_command.alt; - - switch(next_nonnav_command.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(); + extern RC_Channel *rc_ch[NUM_CHANNELS]; + channel = channel - 1; + if (channel < 5 || channel > 8) { + // not allowed + return; } + event_state.rc_channel = channel; + event_state.type = EVENT_TYPE_SERVO; + + event_state.start_time_ms = 0; + event_state.delay_ms = delay_time * 500UL; + event_state.repeat = repeat * 2; + event_state.servo_value = servo_value; + event_state.undo_value = rc_ch[channel]->radio_trim; + update_events(); } static void do_repeat_relay() { - event_id = RELAY_TOGGLE; - event_timer_ms = 0; - event_delay_ms = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_nonnav_command.alt * 2; + event_state.type = EVENT_TYPE_RELAY; + 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; update_events(); } diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index f863719e7c..901d02aae8 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -86,28 +86,39 @@ void low_battery_event(void) } #endif -static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY -{ - if(event_repeat == 0 || (millis() - event_timer_ms) < event_delay_ms) - return; +//////////////////////////////////////////////////////////////////////////////// +// repeating event control - if (event_repeat > 0) { - event_repeat--; +/* + 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; } - if(event_repeat != 0) { // event_repeat = -1 means repeat forever - event_timer_ms = millis(); + // event_repeat = -1 means repeat forever + if (event_state.repeat != 0) { + event_state.start_time_ms = millis(); - if (event_id >= CH_5 && event_id <= CH_8) { - if(event_repeat%2) { - APM_RC.OutputCh(event_id, event_value); // send to Servos + switch (event_state.type) { + case EVENT_TYPE_SERVO: + APM_RC.enable_out(event_state.rc_channel); + if (event_state.repeat & 1) { + APM_RC.OutputCh(event_state.rc_channel, event_state.undo_value); } else { - APM_RC.OutputCh(event_id, event_undo_value); + APM_RC.OutputCh(event_state.rc_channel, event_state.servo_value); } + break; + + case EVENT_TYPE_RELAY: + relay.toggle(); + break; } - if (event_id == RELAY_TOGGLE) { - relay.toggle(); + if (event_state.repeat > 0) { + event_state.repeat--; } } }