AP_Mount: add HAL_MOUNT_SERVO_ENABLED build option

This commit is contained in:
Randy Mackay 2022-06-14 13:55:10 +09:00 committed by Andrew Tridgell
parent e2106e63bf
commit 4f0ee1276b
3 changed files with 13 additions and 5 deletions

View File

@ -443,8 +443,10 @@ void AP_Mount::init()
// check for servo mounts
if (mount_type == Mount_Type_Servo) {
#if HAL_MOUNT_SERVO_ENABLED
_backends[instance] = new AP_Mount_Servo(*this, state[instance], instance);
_num_instances++;
#endif
#if HAL_SOLO_GIMBAL_ENABLED
// check for MAVLink mounts

View File

@ -1,5 +1,5 @@
#include "AP_Mount_Servo.h"
#if HAL_MOUNT_ENABLED
#if HAL_MOUNT_SERVO_ENABLED
extern const AP_HAL::HAL& hal;
@ -221,4 +221,4 @@ void AP_Mount_Servo::move_servo(uint8_t function_idx, int16_t angle, int16_t ang
int16_t servo_out = closest_limit(angle, angle_min, angle_max);
SRV_Channels::move_servo((SRV_Channel::Aux_servo_function_t)function_idx, servo_out, angle_min, angle_max);
}
#endif // HAL_MOUNT_ENABLED
#endif // HAL_MOUNT_SERVO_ENABLED

View File

@ -3,13 +3,19 @@
*/
#pragma once
#include "AP_Mount_Backend.h"
#ifndef HAL_MOUNT_SERVO_ENABLED
#define HAL_MOUNT_SERVO_ENABLED HAL_MOUNT_ENABLED
#endif
#if HAL_MOUNT_SERVO_ENABLED
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <SRV_Channel/SRV_Channel.h>
#include "AP_Mount_Backend.h"
#if HAL_MOUNT_ENABLED
class AP_Mount_Servo : public AP_Mount_Backend
{
@ -71,4 +77,4 @@ private:
uint32_t _last_check_servo_map_ms; // system time of latest call to check_servo_map function
};
#endif // HAL_MOUNT_ENABLED
#endif // HAL_MOUNT_SERVO_ENABLED