From e8b8cc1a5d0cdabffd414f058d4b814f127f0775 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Jan 2014 16:05:31 +1100 Subject: [PATCH] Copter: converted to AP_ServoRelayEvents --- ArduCopter/ArduCopter.pde | 20 ++------- ArduCopter/GCS_Mavlink.pde | 47 +++++++++++++++++++-- ArduCopter/commands_logic.pde | 78 ++++++----------------------------- ArduCopter/events.pde | 24 +---------- ArduCopter/system.pde | 3 ++ 5 files changed, 65 insertions(+), 107 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 1a8bf3d7fd..fddda50d4f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -115,6 +115,7 @@ #include // Filter library #include // APM FIFO Buffer #include // APM relay +#include #include // Photo or video camera #include // Camera/Antenna mount #include // needed for AHRS build @@ -725,22 +726,6 @@ static int16_t yaw_look_at_heading_slew; -//////////////////////////////////////////////////////////////////////////////// -// Repeat Mission Scripting Command -//////////////////////////////////////////////////////////////////////////////// -// The type of repeating event - Toggle a servo channel, Toggle the APM1 relay, etc -static uint8_t event_id; -// Used to manage the timimng of repeating events -static uint32_t event_timer; -// How long to delay the next firing of event in millis -static uint16_t event_delay; -// how many times to fire : 0 = forever, 1 = do once, 2 = do twice -static int16_t event_repeat; -// per command value, such as PWM for servos -static int16_t event_value; -// the stored value used to undo commands - such as original PWM command -static int16_t event_undo_value; - //////////////////////////////////////////////////////////////////////////////// // Delay Mission Scripting Command //////////////////////////////////////////////////////////////////////////////// @@ -786,6 +771,9 @@ static uint8_t auto_trim_counter; // Reference to the relay object (APM1 -> PORTL 2) (APM2 -> PORTB 7) static AP_Relay relay; +// handle repeated servo and relay events +static AP_ServoRelayEvents ServoRelayEvents(relay); + //Reference to the camera object (it uses the relay object inside it) #if CAMERA == ENABLED static AP_Camera camera(&relay); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index e843dd9ed2..a6869bc311 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1216,8 +1216,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_SET_SERVO: - servo_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: @@ -1374,8 +1393,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; @@ -1666,8 +1695,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/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index b16680b547..75828452ad 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -86,20 +86,22 @@ static void process_now_command() do_set_home(); break; - case MAV_CMD_DO_SET_SERVO: // 183 - do_set_servo(); + case MAV_CMD_DO_SET_SERVO: + ServoRelayEvents.do_set_servo(command_cond_queue.p1, command_cond_queue.alt); break; - - case MAV_CMD_DO_SET_RELAY: // 181 - do_set_relay(); + + case MAV_CMD_DO_SET_RELAY: + ServoRelayEvents.do_set_relay(command_cond_queue.p1, command_cond_queue.alt); break; - - case MAV_CMD_DO_REPEAT_SERVO: // 184 - do_repeat_servo(); + + case MAV_CMD_DO_REPEAT_SERVO: + ServoRelayEvents.do_repeat_servo(command_cond_queue.p1, command_cond_queue.alt, + command_cond_queue.lat, command_cond_queue.lng); break; - - case MAV_CMD_DO_REPEAT_RELAY: // 182 - do_repeat_relay(); + + case MAV_CMD_DO_REPEAT_RELAY: + ServoRelayEvents.do_repeat_relay(command_cond_queue.p1, command_cond_queue.alt, + command_cond_queue.lat); break; case MAV_CMD_DO_SET_ROI: // 201 @@ -898,60 +900,6 @@ static void do_set_home() } } -static void do_set_servo() -{ - servo_write(command_cond_queue.p1 - 1, command_cond_queue.alt); -} - -static void do_set_relay() -{ - if (command_cond_queue.p1 == 1) { - relay.on(0); - } else if (command_cond_queue.p1 == 0) { - relay.off(0); - }else{ - relay.toggle(0); - } -} - -static void do_repeat_servo() -{ - event_id = command_cond_queue.p1 - 1; - - if(command_cond_queue.p1 >= CH_5 + 1 && command_cond_queue.p1 <= CH_8 + 1) { - - event_timer = 0; - event_value = command_cond_queue.alt; - event_repeat = command_cond_queue.lat * 2; - event_delay = command_cond_queue.lng * 500.0f; // /2 (half cycle time) * 1000 (convert to milliseconds) - - switch(command_cond_queue.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(); - } -} - -static void do_repeat_relay() -{ - event_id = RELAY_TOGGLE; - event_timer = 0; - event_delay = command_cond_queue.lat * 500.0f; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = command_cond_queue.alt * 2; - update_events(); -} - // do_roi - starts actions required by MAV_CMD_NAV_ROI // this involves either moving the camera to point at the ROI (region of interest) // and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index ce66ffe884..7782f057e3 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -307,28 +307,8 @@ static void failsafe_gcs_off_event(void) Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED); } -static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY +static void update_events() { - if(event_repeat == 0 || (millis() - event_timer) < event_delay) - return; - - 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); - } - if (event_repeat > 0) { - event_repeat--; - } - } + ServoRelayEvents.update_events(); } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 9a1685c7ce..1bdf46a866 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -125,6 +125,9 @@ static void init_ardupilot() BoardConfig.init(); + // FIX: this needs to be the inverse motors mask + ServoRelayEvents.set_channel_mask(0xFFF0); + relay.init(); bool enable_external_leds = true;