From 07df203d1959db078df31fa018bce9392178573f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 24 Jul 2020 18:30:21 +0100 Subject: [PATCH] Sub: make mount/gimbal inclusion configurable per-board --- ArduSub/ArduSub.cpp | 4 ++-- ArduSub/Parameters.cpp | 2 +- ArduSub/Sub.h | 2 +- ArduSub/commands_logic.cpp | 2 +- ArduSub/config.h | 8 -------- ArduSub/control_auto.cpp | 8 ++++---- ArduSub/flight_mode.cpp | 4 ++-- ArduSub/joystick.cpp | 2 +- ArduSub/system.cpp | 2 +- 9 files changed, 13 insertions(+), 21 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index ba455bdff3..7b5150cccd 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -43,7 +43,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { #if AC_FENCE == ENABLED SCHED_TASK_CLASS(AC_Fence, &sub.fence, update, 10, 100), #endif -#if MOUNT == ENABLED +#if HAL_MOUNT_ENABLED SCHED_TASK_CLASS(AP_Mount, &sub.camera_mount, update, 50, 75), #endif #if CAMERA == ENABLED @@ -126,7 +126,7 @@ void Sub::fast_loop() // check if we've reached the surface or bottom update_surface_and_bottom_detector(); -#if MOUNT == ENABLED +#if HAL_MOUNT_ENABLED // camera mount's fast update camera_mount.update_fast(); #endif diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 4271329482..04e2751149 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -468,7 +468,7 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp GOBJECT(ahrs, "AHRS_", AP_AHRS), -#if MOUNT == ENABLED +#if HAL_MOUNT_ENABLED // @Group: MNT // @Path: ../libraries/AP_Mount/AP_Mount.cpp GOBJECT(camera_mount, "MNT", AP_Mount), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 8ea20abc78..197ec12da9 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -359,7 +359,7 @@ private: #endif // Camera/Antenna mount tracking and stabilisation stuff -#if MOUNT == ENABLED +#if HAL_MOUNT_ENABLED AP_Mount camera_mount; #endif diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 577e09ca05..91212ac962 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -789,7 +789,7 @@ void Sub::do_roi(const AP_Mission::Mission_Command& cmd) // point the camera to a specified angle void Sub::do_mount_control(const AP_Mission::Mission_Command& cmd) { -#if MOUNT == ENABLED +#if HAL_MOUNT_ENABLED camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw); #endif } diff --git a/ArduSub/config.h b/ArduSub/config.h index d41188e700..9dd64ee8f4 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -138,14 +138,6 @@ # define CAMERA DISABLED #endif -////////////////////////////////////////////////////////////////////////////// -// MOUNT (ANTENNA OR CAMERA) -// -#ifndef MOUNT -# define MOUNT ENABLED -#endif - - ////////////////////////////////////////////////////////////////////////////// // Flight mode definitions // diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 67042097a5..fd62fc49e7 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -536,14 +536,14 @@ void Sub::set_auto_yaw_roi(const Location &roi_location) if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) { // set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command set_auto_yaw_mode(get_default_auto_yaw_mode(false)); -#if MOUNT == ENABLED +#if HAL_MOUNT_ENABLED // switch off the camera tracking if enabled if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { camera_mount.set_mode_to_default(); } -#endif // MOUNT == ENABLED +#endif // HAL_MOUNT_ENABLED } else { -#if MOUNT == ENABLED +#if HAL_MOUNT_ENABLED // check if mount type requires us to rotate the quad if (!camera_mount.has_pan_control()) { roi_WP = pv_location_to_vector(roi_location); @@ -562,7 +562,7 @@ void Sub::set_auto_yaw_roi(const Location &roi_location) // if we have no camera mount aim the quad at the location roi_WP = pv_location_to_vector(roi_location); set_auto_yaw_mode(AUTO_YAW_ROI); -#endif // MOUNT == ENABLED +#endif // HAL_MOUNT_ENABLED } } diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index 1e1077a998..2fed178399 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -165,9 +165,9 @@ void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_ if (mission.state() == AP_Mission::MISSION_RUNNING) { mission.stop(); } -#if MOUNT == ENABLED +#if HAL_MOUNT_ENABLED camera_mount.set_mode_to_default(); -#endif // MOUNT == ENABLED +#endif // HAL_MOUNT_ENABLED } } diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index ae20a983db..d77268e7d4 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -182,7 +182,7 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) break; case JSButton::button_function_t::k_mount_center: -#if MOUNT == ENABLED +#if HAL_MOUNT_ENABLED camera_mount.set_angle_targets(0, 0, 0); // for some reason the call to set_angle_targets changes the mode to mavlink targeting! camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING); diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index df5dd4f169..5b9e035674 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -100,7 +100,7 @@ void Sub::init_ardupilot() init_optflow(); #endif -#if MOUNT == ENABLED +#if HAL_MOUNT_ENABLED // initialise camera mount camera_mount.init(); // This step ncessary so the servo is properly initialized