diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 62d94b0113..d5af43ab43 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1,6 +1,9 @@ #include #include #include "AP_Mount.h" + +#if HAL_MOUNT_ENABLED + #include "AP_Mount_Backend.h" #include "AP_Mount_Servo.h" #include "AP_Mount_SoloGimbal.h" @@ -439,14 +442,12 @@ void AP_Mount::init() _backends[instance] = new AP_Mount_Servo(*this, state[instance], instance); _num_instances++; -#if AP_AHRS_NAVEKF_AVAILABLE -#if !HAL_MINIMIZE_FEATURES +#if HAL_SOLO_GIMBAL_ENABLED // check for MAVLink mounts } else if (mount_type == Mount_Type_SoloGimbal) { _backends[instance] = new AP_Mount_SoloGimbal(*this, state[instance], instance); _num_instances++; -#endif // HAL_MINIMIZE_FEATURES -#endif // AP_AHRS_NAVEKF_AVAILABLE +#endif // HAL_SOLO_GIMBAL_ENABLED // check for Alexmos mounts } else if (mount_type == Mount_Type_Alexmos) { @@ -746,3 +747,5 @@ AP_Mount *mount() } }; + +#endif /* HAL_MOUNT_ENABLED */ diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 85d4740f66..720acdb234 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -19,6 +19,19 @@ ************************************************************/ #pragma once +#include +#include + +#ifndef HAL_MOUNT_ENABLED +#define HAL_MOUNT_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +#ifndef HAL_SOLO_GIMBAL_ENABLED +#define HAL_SOLO_GIMBAL_ENABLED HAL_MOUNT_ENABLED && AP_AHRS_NAVEKF_AVAILABLE && BOARD_FLASH_SIZE > 1024 +#endif + +#if HAL_MOUNT_ENABLED + #include #include #include @@ -193,3 +206,5 @@ private: namespace AP { AP_Mount *mount(); }; + +#endif // HAL_MOUNT_ENABLED \ No newline at end of file diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index ab3af2c903..bdff3e30ed 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -1,4 +1,5 @@ #include "AP_Mount_Alexmos.h" +#if HAL_MOUNT_ENABLED #include #include @@ -293,3 +294,4 @@ void AP_Mount_Alexmos::read_incoming() } } } +#endif // HAL_MOUNT_ENABLED \ No newline at end of file diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.h b/libraries/AP_Mount/AP_Mount_Alexmos.h index c6dcb94494..713319d9b8 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.h +++ b/libraries/AP_Mount/AP_Mount_Alexmos.h @@ -4,6 +4,7 @@ #pragma once #include "AP_Mount.h" +#if HAL_MOUNT_ENABLED #include #include #include @@ -299,3 +300,4 @@ private: // confirmed that last command was ok bool _last_command_confirmed : 1; }; +#endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 0bb004d344..96fc49d21a 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -1,4 +1,5 @@ #include "AP_Mount_Backend.h" +#if HAL_MOUNT_ENABLED #include extern const AP_HAL::HAL& hal; @@ -208,3 +209,5 @@ bool AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vec } return true; } + +#endif // HAL_MOUNT_ENABLED \ No newline at end of file diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 24a26d6e78..ce5fdef05d 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -21,6 +21,7 @@ #include #include "AP_Mount.h" +#if HAL_MOUNT_ENABLED #include class AP_Mount_Backend @@ -125,3 +126,5 @@ private: void rate_input_rad(float &out, const RC_Channel *ch, float min, float max) const; }; + +#endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 75414abac4..d0be0c4ef6 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -1,4 +1,5 @@ #include "AP_Mount_SToRM32.h" +#if HAL_MOUNT_ENABLED #include #include #include @@ -157,3 +158,4 @@ void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, fl // store time of send _last_send = AP_HAL::millis(); } +#endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.h b/libraries/AP_Mount/AP_Mount_SToRM32.h index c0779dfb79..75b639b078 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32.h @@ -10,6 +10,7 @@ #include #include #include "AP_Mount_Backend.h" +#if HAL_MOUNT_ENABLED #define AP_MOUNT_STORM32_RESEND_MS 1000 // resend angle targets to gimbal once per second #define AP_MOUNT_STORM32_SEARCH_MS 60000 // search for gimbal for 1 minute after startup @@ -51,3 +52,4 @@ private: mavlink_channel_t _chan; // mavlink channel used to communicate with gimbal. Currently hard-coded to Telem2 uint32_t _last_send; // system time of last do_mount_control sent to gimbal }; +#endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 777b8764e6..41ab148f00 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -1,4 +1,5 @@ #include "AP_Mount_SToRM32_serial.h" +#if HAL_MOUNT_ENABLED #include #include #include @@ -284,3 +285,4 @@ void AP_Mount_SToRM32_serial::parse_reply() { break; } } +#endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h index 5df9a85fd4..8d5b39c2f2 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h @@ -10,6 +10,7 @@ #include #include #include "AP_Mount_Backend.h" +#if HAL_MOUNT_ENABLED #define AP_MOUNT_STORM32_SERIAL_RESEND_MS 1000 // resend angle targets to gimbal once per second @@ -147,3 +148,4 @@ private: // keep the last _current_angle values Vector3l _current_angle; }; +#endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 14542d1c74..c8df1c2648 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -1,4 +1,5 @@ #include "AP_Mount_Servo.h" +#if HAL_MOUNT_ENABLED #include extern const AP_HAL::HAL& hal; @@ -209,3 +210,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 diff --git a/libraries/AP_Mount/AP_Mount_Servo.h b/libraries/AP_Mount/AP_Mount_Servo.h index 669cbfa26a..f5ce1b28c4 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.h +++ b/libraries/AP_Mount/AP_Mount_Servo.h @@ -9,6 +9,7 @@ #include #include #include "AP_Mount_Backend.h" +#if HAL_MOUNT_ENABLED class AP_Mount_Servo : public AP_Mount_Backend { @@ -70,3 +71,4 @@ private: uint32_t _last_check_servo_map_ms; // system time of latest call to check_servo_map function }; +#endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index f959dda443..987ea5638d 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -1,8 +1,8 @@ #include #include -#if AP_AHRS_NAVEKF_AVAILABLE - #include "AP_Mount_SoloGimbal.h" +#if HAL_SOLO_GIMBAL_ENABLED + #include "SoloGimbal.h" #include #include @@ -158,4 +158,4 @@ void AP_Mount_SoloGimbal::send_gimbal_report(mavlink_channel_t chan) { } -#endif // AP_AHRS_NAVEKF_AVAILABLE +#endif // HAL_SOLO_GIMBAL_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.h b/libraries/AP_Mount/AP_Mount_SoloGimbal.h index e41a997c52..d8238f0fa0 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.h +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.h @@ -7,12 +7,12 @@ #include #include -#if AP_AHRS_NAVEKF_AVAILABLE +#include "AP_Mount_Backend.h" +#if HAL_SOLO_GIMBAL_ENABLED #include #include #include #include -#include "AP_Mount_Backend.h" #include "SoloGimbal.h" @@ -60,4 +60,4 @@ private: SoloGimbal _gimbal; }; -#endif // AP_AHRS_NAVEKF_AVAILABLE +#endif // HAL_SOLO_GIMBAL_ENABLED diff --git a/libraries/AP_Mount/SoloGimbal.cpp b/libraries/AP_Mount/SoloGimbal.cpp index 242c2e0a1e..07467202ae 100644 --- a/libraries/AP_Mount/SoloGimbal.cpp +++ b/libraries/AP_Mount/SoloGimbal.cpp @@ -1,9 +1,9 @@ #include #include -#if AP_AHRS_NAVEKF_AVAILABLE - #include "SoloGimbal.h" +#if HAL_SOLO_GIMBAL_ENABLED + #include #include #include @@ -529,4 +529,4 @@ void SoloGimbal::joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vect ang_vel.z = sin_theta*joint_rates.x+cos_theta*cos_phi*joint_rates.z; } -#endif // AP_AHRS_NAVEKF_AVAILABLE +#endif // HAL_SOLO_GIMBAL_ENABLED diff --git a/libraries/AP_Mount/SoloGimbal.h b/libraries/AP_Mount/SoloGimbal.h index e25567ea88..9cd2b62cd7 100644 --- a/libraries/AP_Mount/SoloGimbal.h +++ b/libraries/AP_Mount/SoloGimbal.h @@ -8,9 +8,8 @@ #include #include -#if AP_AHRS_NAVEKF_AVAILABLE - #include "AP_Mount.h" +#if HAL_SOLO_GIMBAL_ENABLED #include "SoloGimbalEKF.h" #include #include @@ -146,4 +145,4 @@ private: Vector3f _log_del_vel; }; -#endif // AP_AHRS_NAVEKF_AVAILABLE +#endif // HAL_SOLO_GIMBAL_ENABLED diff --git a/libraries/AP_Mount/SoloGimbalEKF.cpp b/libraries/AP_Mount/SoloGimbalEKF.cpp index c288cfd989..5cf366da31 100644 --- a/libraries/AP_Mount/SoloGimbalEKF.cpp +++ b/libraries/AP_Mount/SoloGimbalEKF.cpp @@ -9,6 +9,7 @@ #endif #include "SoloGimbalEKF.h" +#if HAL_SOLO_GIMBAL_ENABLED #include #include #include @@ -961,4 +962,5 @@ bool SoloGimbalEKF::getStatus() const float run_time = AP_HAL::millis() - StartTime_ms; return YawAligned && (run_time > 15000); } +#endif // HAL_SOLO_GIMBAL_ENABLED diff --git a/libraries/AP_Mount/SoloGimbalEKF.h b/libraries/AP_Mount/SoloGimbalEKF.h index 9425919c44..05a93d856d 100644 --- a/libraries/AP_Mount/SoloGimbalEKF.h +++ b/libraries/AP_Mount/SoloGimbalEKF.h @@ -21,7 +21,8 @@ #include #include //#include - +#include "AP_Mount.h" +#if HAL_SOLO_GIMBAL_ENABLED #include class SoloGimbalEKF @@ -125,3 +126,4 @@ private: // Force symmmetry and non-negative diagonals on state covarinace matrix void fixCovariance(); }; +#endif // HAL_SOLO_GIMBAL_ENABLED diff --git a/libraries/AP_Mount/SoloGimbal_Parameters.cpp b/libraries/AP_Mount/SoloGimbal_Parameters.cpp index 202058c15e..4864a59acd 100644 --- a/libraries/AP_Mount/SoloGimbal_Parameters.cpp +++ b/libraries/AP_Mount/SoloGimbal_Parameters.cpp @@ -1,4 +1,5 @@ #include "SoloGimbal_Parameters.h" +#if HAL_SOLO_GIMBAL_ENABLED #include #include #include @@ -292,3 +293,5 @@ bool SoloGimbal_Parameters::flashing() { return _flashing_step != GMB_PARAM_NOT_FLASHING; } + +#endif // HAL_SOLO_GIMBAL_ENABLED \ No newline at end of file diff --git a/libraries/AP_Mount/SoloGimbal_Parameters.h b/libraries/AP_Mount/SoloGimbal_Parameters.h index 0188c6ba65..f46a87b4a7 100644 --- a/libraries/AP_Mount/SoloGimbal_Parameters.h +++ b/libraries/AP_Mount/SoloGimbal_Parameters.h @@ -2,6 +2,8 @@ #include #include #include +#include "AP_Mount.h" +#if HAL_SOLO_GIMBAL_ENABLED enum gmb_param_state_t { GMB_PARAMSTATE_NOT_YET_READ=0, // parameter has yet to be initialized @@ -88,3 +90,5 @@ private: mavlink_channel_t _chan; }; + +#endif // HAL_SOLO_GIMBAL_ENABLED \ No newline at end of file