From 681d28838f78e538f418cd0ed11b5abbc3b59e02 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 9 Jan 2015 05:12:08 +0900 Subject: [PATCH] Plane: integrate mount frontend-backend restructure initialise mount on startup use mount.has_pan_control method remove calls to unimplemented mount.configure_cmd remove call to update_mount_type which is now handled by mount lib --- ArduPlane/ArduPlane.pde | 21 ++------------------- ArduPlane/GCS_Mavlink.pde | 2 +- ArduPlane/Parameters.h | 2 +- ArduPlane/Parameters.pde | 6 ------ ArduPlane/commands_logic.pde | 12 ++---------- ArduPlane/config.h | 5 ----- ArduPlane/system.pde | 3 +++ 7 files changed, 9 insertions(+), 42 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index cdc6efc98e..6db436447f 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -739,13 +739,7 @@ static uint16_t mainLoop_count; #if MOUNT == ENABLED // current_loc uses the baro/gps soloution for altitude rather than gps only. // mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether? -static AP_Mount camera_mount(¤t_loc, ahrs, 0); -#endif - -#if MOUNT2 == ENABLED -// current_loc uses the baro/gps soloution for altitude rather than gps only. -// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether? -static AP_Mount camera_mount2(¤t_loc, ahrs, 1); +static AP_Mount camera_mount(ahrs, current_loc); #endif //////////////////////////////////////////////////////////////////////////////// @@ -915,11 +909,7 @@ static void update_speed_height(void) static void update_mount(void) { #if MOUNT == ENABLED - camera_mount.update_mount_position(); -#endif - -#if MOUNT2 == ENABLED - camera_mount2.update_mount_position(); + camera_mount.update(); #endif #if CAMERA == ENABLED @@ -1010,13 +1000,6 @@ static void update_aux(void) if (!px4io_override_enabled) { RC_Channel_aux::enable_aux_servos(); } - -#if MOUNT == ENABLED - camera_mount.update_mount_type(); -#endif -#if MOUNT2 == ENABLED - camera_mount2.update_mount_type(); -#endif } static void one_second_loop() diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index e246e29d3f..36703a7133 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1020,7 +1020,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } } else { // send the command to the camera mount - camera_mount.set_roi_cmd(&roi_loc); + camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 856f2abfea..d414c98941 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -208,7 +208,7 @@ public: // k_param_camera = 160, k_param_camera_mount, - k_param_camera_mount2, + k_param_camera_mount2, // unused // // Battery monitoring parameters diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 2bb45f7498..9e3557269d 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -1112,12 +1112,6 @@ const AP_Param::Info var_info[] PROGMEM = { GOBJECT(camera_mount, "MNT_", AP_Mount), #endif -#if MOUNT2 == ENABLED - // @Group: MNT2_ - // @Path: ../libraries/AP_Mount/AP_Mount.cpp - GOBJECT(camera_mount2, "MNT2_", AP_Mount), -#endif - // @Group: BATT_ // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp GOBJECT(battery, "BATT", AP_BattMonitor), diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 07a981973c..d76a6b14c3 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -176,18 +176,10 @@ start_command(const AP_Mission::Mission_Command& cmd) camera_mount.set_mode_to_default(); } } else { - // send the command to the camera mount - camera_mount.set_roi_cmd(&cmd.content.location); + // set mount's target location + camera_mount.set_roi_target(cmd.content.location); } break; - - case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| - camera_mount.configure_cmd(); - break; - - case MAV_CMD_DO_MOUNT_CONTROL: // Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty| - camera_mount.control_cmd(); - break; #endif } diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 573407f741..d1e96d9162 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -282,11 +282,6 @@ # define MOUNT ENABLED #endif -// second mount, can for example be used to keep an antenna pointed at the home position -#ifndef MOUNT2 - # define MOUNT2 DISABLED -#endif - ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // FLIGHT AND NAVIGATION CONTROL diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index afc667951b..c6f79de43a 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -186,6 +186,9 @@ static void init_ardupilot() relay.init(); + // initialise camera mount + camera_mount.init(); + #if FENCE_TRIGGERED_PIN > 0 hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); hal.gpio->write(FENCE_TRIGGERED_PIN, 0);