Sub: make mount/gimbal inclusion configurable per-board

This commit is contained in:
Andy Piper 2020-07-24 18:30:21 +01:00 committed by Andrew Tridgell
parent 6d87d61c7b
commit 07df203d19
9 changed files with 13 additions and 21 deletions

View File

@ -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

View File

@ -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),

View File

@ -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

View File

@ -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
}

View File

@ -138,14 +138,6 @@
# define CAMERA DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// MOUNT (ANTENNA OR CAMERA)
//
#ifndef MOUNT
# define MOUNT ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Flight mode definitions
//

View File

@ -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
}
}

View File

@ -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
}
}

View File

@ -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);

View File

@ -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