mirror of https://github.com/ArduPilot/ardupilot
Sub: make mount/gimbal inclusion configurable per-board
This commit is contained in:
parent
6d87d61c7b
commit
07df203d19
|
@ -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
|
||||
|
|
|
@ -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),
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
|
|
@ -138,14 +138,6 @@
|
|||
# define CAMERA DISABLED
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// MOUNT (ANTENNA OR CAMERA)
|
||||
//
|
||||
#ifndef MOUNT
|
||||
# define MOUNT ENABLED
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Flight mode definitions
|
||||
//
|
||||
|
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue