Copter: make make/gimbal configurable on a per-board basis

This commit is contained in:
Andy Piper 2020-07-24 18:02:40 +01:00 committed by Andrew Tridgell
parent f7f5880179
commit 54c3d07547
10 changed files with 18 additions and 26 deletions

View File

@ -146,7 +146,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180),
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550),
#if MOUNT == ENABLED
#if HAL_MOUNT_ENABLED
SCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75),
#endif
#if CAMERA == ENABLED
@ -260,7 +260,7 @@ void Copter::fast_loop()
// check if we've landed or crashed
update_land_and_crash_detectors();
#if MOUNT == ENABLED
#if HAL_MOUNT_ENABLED
// camera mount's fast update
camera_mount.update_fast();
#endif

View File

@ -133,9 +133,9 @@
#if PROXIMITY_ENABLED == ENABLED
# include <AP_Proximity/AP_Proximity.h>
#endif
#if MOUNT == ENABLED
#include <AP_Mount/AP_Mount.h>
#endif
#include <AP_Mount/AP_Mount.h>
#if CAMERA == ENABLED
# include <AP_Camera/AP_Camera.h>
#endif
@ -489,7 +489,7 @@ private:
#endif
// Camera/Antenna mount tracking and stabilisation stuff
#if MOUNT == ENABLED
#if HAL_MOUNT_ENABLED
AP_Mount camera_mount;
#endif

View File

@ -644,7 +644,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t
{
// if the mount doesn't do pan control then yaw the entire vehicle instead:
switch (packet.command) {
#if MOUNT == ENABLED
#if HAL_MOUNT_ENABLED
case MAV_CMD_DO_MOUNT_CONTROL:
if (!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
@ -919,7 +919,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg)
{
switch (msg.msgid) {
#if MOUNT == ENABLED
#if HAL_MOUNT_ENABLED
case MAVLINK_MSG_ID_MOUNT_CONTROL:
if (!copter.camera_mount.has_pan_control()) {
// if the mount doesn't do pan control then yaw the entire vehicle instead:

View File

@ -548,7 +548,7 @@ const AP_Param::Info Copter::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

@ -363,7 +363,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
break;
case AUX_FUNC::RETRACT_MOUNT:
#if MOUNT == ENABLE
#if HAL_MOUNT_ENABLED
switch (ch_flag) {
case AuxSwitchPos::HIGH:
copter.camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT);

View File

@ -140,14 +140,14 @@ void Mode::AutoYaw::set_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
auto_yaw.set_mode_to_default(false);
#if MOUNT == ENABLED
#if HAL_MOUNT_ENABLED
// switch off the camera tracking if enabled
if (copter.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
copter.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 (!copter.camera_mount.has_pan_control()) {
if (roi_location.get_vector_from_origin_NEU(roi)) {
@ -168,7 +168,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
if (roi_location.get_vector_from_origin_NEU(roi)) {
auto_yaw.set_mode(AUTO_YAW_ROI);
}
#endif // MOUNT == ENABLED
#endif // HAL_MOUNT_ENABLED
}
}

View File

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

View File

@ -328,9 +328,9 @@ void Copter::exit_mode(Mode *&old_flightmode,
if (mode_auto.mission.state() == AP_Mission::MISSION_RUNNING) {
mode_auto.mission.stop();
}
#if MOUNT == ENABLED
#if HAL_MOUNT_ENABLED
camera_mount.set_mode_to_default();
#endif // MOUNT == ENABLED
#endif // HAL_MOUNT_ENABLED
}
#endif

View File

@ -1420,7 +1420,7 @@ void ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
// point the camera to a specified angle
void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
{
#if MOUNT == ENABLED
#if HAL_MOUNT_ENABLED
if (!copter.camera_mount.has_pan_control()) {
auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,0);
}

View File

@ -130,7 +130,7 @@ void Copter::init_ardupilot()
// init the optical flow sensor
init_optflow();
#if MOUNT == ENABLED
#if HAL_MOUNT_ENABLED
// initialise camera mount
camera_mount.init();
#endif