diff --git a/libraries/AC_CustomControl/AC_CustomControl.cpp b/libraries/AC_CustomControl/AC_CustomControl.cpp index a5df09f11b8e8..9a291887b5b06 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl.cpp @@ -4,7 +4,6 @@ #if AP_CUSTOMCONTROL_ENABLED - #include "AC_CustomControl_Backend.h" // #include "AC_CustomControl_Empty.h" #include "AC_CustomControl_PID.h" @@ -123,30 +122,30 @@ void AC_CustomControl::set_custom_controller(bool enabled) // don't allow accidental main controller reset without active custom controller if (_controller_type == CustomControlType::CONT_NONE) { - gcs().send_text(MAV_SEVERITY_INFO, "Custom controller is not enabled"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Custom controller is not enabled"); return; } // controller type is out of range if (_controller_type > CUSTOMCONTROL_MAX_TYPES) { - gcs().send_text(MAV_SEVERITY_INFO, "Custom controller type is out of range"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Custom controller type is out of range"); return; } // backend is not created if (_backend == nullptr) { - gcs().send_text(MAV_SEVERITY_INFO, "Reboot to enable selected custom controller"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Reboot to enable selected custom controller"); return; } if (_custom_controller_mask == 0 && enabled) { - gcs().send_text(MAV_SEVERITY_INFO, "Axis mask is not set"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Axis mask is not set"); return; } // reset main controller if (!enabled) { - gcs().send_text(MAV_SEVERITY_INFO, "Custom controller is OFF"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Custom controller is OFF"); // don't reset if the empty backend is selected if (_controller_type > CustomControlType::CONT_EMPTY) { reset_main_att_controller(); @@ -156,7 +155,7 @@ void AC_CustomControl::set_custom_controller(bool enabled) if (enabled && _controller_type > CustomControlType::CONT_NONE) { // reset custom controller filter, integrator etc. _backend->reset(); - gcs().send_text(MAV_SEVERITY_INFO, "Custom controller is ON"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Custom controller is ON"); } _custom_controller_active = enabled; @@ -193,4 +192,4 @@ void AC_CustomControl::set_notch_sample_rate(float sample_rate) #endif } -#endif +#endif // AP_CUSTOMCONTROL_ENABLED diff --git a/libraries/AC_CustomControl/AC_CustomControl.h b/libraries/AC_CustomControl/AC_CustomControl.h index e4405b3c28a77..93f33a7300a90 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.h +++ b/libraries/AC_CustomControl/AC_CustomControl.h @@ -3,6 +3,10 @@ /// @file AC_CustomControl.h /// @brief ArduCopter custom control library +#include "AC_CustomControl_config.h" + +#if AP_CUSTOMCONTROL_ENABLED + #include #include #include @@ -10,8 +14,6 @@ #include #include -#if AP_CUSTOMCONTROL_ENABLED - #ifndef CUSTOMCONTROL_MAX_TYPES #define CUSTOMCONTROL_MAX_TYPES 2 #endif @@ -72,4 +74,4 @@ class AC_CustomControl { AC_CustomControl_Backend *_backend; }; -#endif +#endif // AP_CUSTOMCONTROL_ENABLED diff --git a/libraries/AC_CustomControl/AC_CustomControl_Backend.h b/libraries/AC_CustomControl/AC_CustomControl_Backend.h index 5355ef9cbc18d..391353585ae4f 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Backend.h +++ b/libraries/AC_CustomControl/AC_CustomControl_Backend.h @@ -1,9 +1,11 @@ #pragma once -#include "AC_CustomControl.h" +#include "AC_CustomControl_config.h" #if AP_CUSTOMCONTROL_ENABLED +#include "AC_CustomControl.h" + class AC_CustomControl_Backend { public: @@ -34,4 +36,4 @@ class AC_CustomControl_Backend AC_CustomControl& _frontend; }; -#endif +#endif // AP_CUSTOMCONTROL_ENABLED diff --git a/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp b/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp index 5c0550b0898d9..7b7f0d5dcfbff 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp @@ -1,6 +1,8 @@ -#include "AC_CustomControl_Empty.h" +#include "AC_CustomControl_config.h" + +#if AP_CUSTOMCONTROL_EMPTY_ENABLED -#if CUSTOMCONTROL_EMPTY_ENABLED +#include "AC_CustomControl_Empty.h" #include @@ -57,7 +59,7 @@ Vector3f AC_CustomControl_Empty::update(void) // arducopter main attitude controller already ran // we don't need to do anything else - gcs().send_text(MAV_SEVERITY_INFO, "empty custom controller working"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "empty custom controller working"); // return what arducopter main controller outputted return Vector3f(_motors->get_roll(), _motors->get_pitch(), _motors->get_yaw()); @@ -69,4 +71,4 @@ void AC_CustomControl_Empty::reset(void) { } -#endif +#endif // AP_CUSTOMCONTROL_EMPTY_ENABLED diff --git a/libraries/AC_CustomControl/AC_CustomControl_Empty.h b/libraries/AC_CustomControl/AC_CustomControl_Empty.h index e3a6374d21624..4628d7c4a20b6 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Empty.h +++ b/libraries/AC_CustomControl/AC_CustomControl_Empty.h @@ -1,12 +1,10 @@ #pragma once -#include "AC_CustomControl_Backend.h" +#include "AC_CustomControl_config.h" -#ifndef CUSTOMCONTROL_EMPTY_ENABLED - #define CUSTOMCONTROL_EMPTY_ENABLED AP_CUSTOMCONTROL_ENABLED -#endif +#if AP_CUSTOMCONTROL_EMPTY_ENABLED -#if CUSTOMCONTROL_EMPTY_ENABLED +#include "AC_CustomControl_Backend.h" class AC_CustomControl_Empty : public AC_CustomControl_Backend { public: @@ -26,4 +24,4 @@ class AC_CustomControl_Empty : public AC_CustomControl_Backend { AP_Float param3; }; -#endif +#endif // AP_CUSTOMCONTROL_EMPTY_ENABLED diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp index 5535d44990940..cd6d00cd2af04 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp @@ -1,8 +1,10 @@ +#include "AC_CustomControl_config.h" + +#if AP_CUSTOMCONTROL_PID_ENABLED + #include "AC_CustomControl_PID.h" #include "AC_AttitudeControl/AC_AttitudeControl_Multi.h" -#if CUSTOMCONTROL_PID_ENABLED - // table of user settable parameters const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = { // @Param: ANG_RLL_P @@ -403,4 +405,4 @@ void AC_CustomControl_PID::set_notch_sample_rate(float sample_rate) #endif } -#endif +#endif // AP_CUSTOMCONTROL_PID_ENABLED diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.h b/libraries/AC_CustomControl/AC_CustomControl_PID.h index 75649a5dd92bf..e0eb0957fb739 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_PID.h +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.h @@ -1,5 +1,9 @@ #pragma once +#include "AC_CustomControl_config.h" + +#if AP_CUSTOMCONTROL_PID_ENABLED + #include #include #include @@ -7,12 +11,6 @@ #include "AC_CustomControl_Backend.h" -#ifndef CUSTOMCONTROL_PID_ENABLED - #define CUSTOMCONTROL_PID_ENABLED AP_CUSTOMCONTROL_ENABLED -#endif - -#if CUSTOMCONTROL_PID_ENABLED - class AC_CustomControl_PID : public AC_CustomControl_Backend { public: AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt); @@ -42,4 +40,4 @@ class AC_CustomControl_PID : public AC_CustomControl_Backend { AC_PID _pid_atti_rate_yaw; }; -#endif +#endif // AP_CUSTOMCONTROL_PID_ENABLED diff --git a/libraries/AC_CustomControl/AC_CustomControl_config.h b/libraries/AC_CustomControl/AC_CustomControl_config.h new file mode 100644 index 0000000000000..368b0b3233046 --- /dev/null +++ b/libraries/AC_CustomControl/AC_CustomControl_config.h @@ -0,0 +1,21 @@ +#pragma once + +#include + +// note boards.py sets this, so the default value in here doesn't +// really matter: +//#ifndef AP_CUSTOMCONTROL_ENABLED +//#define AP_CUSTOMCONTROL_ENABLED 0 +//#endif + +#ifndef AP_CUSTOMCONTROL_BACKEND_DEFAULT_ENABLED +#define AP_CUSTOMCONTROL_BACKEND_DEFAULT_ENABLED AP_CUSTOMCONTROL_ENABLED +#endif + +#ifndef AP_CUSTOMCONTROL_EMPTY_ENABLED +#define AP_CUSTOMCONTROL_EMPTY_ENABLED AP_CUSTOMCONTROL_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_CUSTOMCONTROL_PID_ENABLED +#define AP_CUSTOMCONTROL_PID_ENABLED AP_CUSTOMCONTROL_BACKEND_DEFAULT_ENABLED +#endif