diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 62364fcd2d..714d352a8c 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -79,6 +79,7 @@ #include // Mode Filter from Filter library #include // Mode Filter from Filter library #include // APM relay +#include #include // Camera/Antenna mount #include // Camera triggering #include // MAVLink GCS definitions @@ -293,6 +294,8 @@ static AP_RangeFinder_analog sonar2; // relay support AP_Relay relay; +AP_ServoRelayEvents ServoRelayEvents(relay); + // Camera #if CAMERA == ENABLED static AP_Camera camera(&relay); @@ -467,22 +470,6 @@ static float wp_distance; // Distance between previous and next waypoint. Meters static int32_t wp_totalDistance; -//////////////////////////////////////////////////////////////////////////////// -// repeating event control -//////////////////////////////////////////////////////////////////////////////// -// Flag indicating current event type -static uint8_t event_id; -// when the event was started in ms -static int32_t event_timer; -// how long to delay the next firing of event in millis -static uint16_t event_delay; -// 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 //////////////////////////////////////////////////////////////////////////////// @@ -574,7 +561,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { read_trim_switch, 5, 1000 }, { read_battery, 5, 1000 }, { read_receiver_rssi, 5, 1000 }, - { update_events, 15, 1000 }, + { update_events, 1, 1000 }, { check_usb_mux, 15, 1000 }, { mount_update, 1, 600 }, { gcs_failsafe_check, 5, 600 }, diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 5baf5a59ec..b4f525da44 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -1142,9 +1142,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_SET_SERVO: - hal.rcout->enable_ch(packet.param1 - 1); - hal.rcout->write(packet.param1 - 1, 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: + 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: + if (ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_DO_REPEAT_RELAY: + if (ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { + result = MAV_RESULT_ACCEPTED; + } break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: @@ -1276,9 +1294,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) param1 = tell_command.p1; break; - case MAV_CMD_DO_REPEAT_SERVO: - param4 = tell_command.lng; - case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_REPEAT_SERVO: + 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; @@ -1567,8 +1595,18 @@ 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.alt = packet.param2; diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index 7eae5e67a8..d594249f40 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -66,21 +66,23 @@ static void handle_process_do_command() do_set_home(); break; - case MAV_CMD_DO_SET_SERVO: - do_set_servo(); - break; + case MAV_CMD_DO_SET_SERVO: + ServoRelayEvents.do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt); + break; - case MAV_CMD_DO_SET_RELAY: - do_set_relay(); - break; + case MAV_CMD_DO_SET_RELAY: + ServoRelayEvents.do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt); + break; - case MAV_CMD_DO_REPEAT_SERVO: - do_repeat_servo(); - break; + case MAV_CMD_DO_REPEAT_SERVO: + 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(); - break; + case MAV_CMD_DO_REPEAT_RELAY: + ServoRelayEvents.do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt, + next_nonnav_command.lat); + break; #if CAMERA == ENABLED case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| @@ -366,47 +368,6 @@ static void do_set_home() } } -static void do_set_servo() -{ - hal.rcout->enable_ch(next_nonnav_command.p1 - 1); - hal.rcout->write(next_nonnav_command.p1 - 1, next_nonnav_command.alt); -} - -static void do_set_relay() -{ - if (next_nonnav_command.p1 == 1) { - relay.on(0); - } else if (next_nonnav_command.p1 == 0) { - relay.off(0); - }else{ - relay.toggle(0); - } -} - -static void do_repeat_servo() -{ - event_id = next_nonnav_command.p1 - 1; - - if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) { - event_timer = 0; - event_delay = 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; - event_undo_value = RC_Channel::rc_channel(next_nonnav_command.p1-1)->radio_trim; - update_events(); - } -} - -static void do_repeat_relay() -{ - event_id = RELAY_TOGGLE; - event_timer = 0; - event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_nonnav_command.alt * 2; - update_events(); -} - - // do_take_picture - take a picture with the camera library static void do_take_picture() { diff --git a/APMrover2/events.pde b/APMrover2/events.pde index da008c2cb7..f4a07313df 100644 --- a/APMrover2/events.pde +++ b/APMrover2/events.pde @@ -1,28 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY +static void update_events(void) { - if(event_repeat == 0 || (millis() - event_timer) < event_delay) - return; - - if (event_repeat > 0){ - event_repeat --; - } - - if(event_repeat != 0) { // event_repeat = -1 means repeat forever - event_timer = millis(); - - if (event_id >= CH_5 && event_id <= CH_8) { - if(event_repeat%2) { - hal.rcout->write(event_id, event_value); // send to Servos - } else { - hal.rcout->write(event_id, event_undo_value); - } - } - - if (event_id == RELAY_TOGGLE) { - relay.toggle(0); - } - } + ServoRelayEvents.update_events(); } diff --git a/APMrover2/system.pde b/APMrover2/system.pde index d1710a3367..888e591cf6 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -106,6 +106,8 @@ static void init_ardupilot() BoardConfig.init(); + ServoRelayEvents.set_channel_mask(0xFFF0); + set_control_channels(); // after parameter load setup correct baud rate on uartA