diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0827e2a09ca9e..bb468a39939a5 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -77,12 +77,6 @@ #include "defines.h" #include "config.h" -#if FRAME_CONFIG == HELI_FRAME - #define AC_AttitudeControl_t AC_AttitudeControl_Heli -#else - #define AC_AttitudeControl_t AC_AttitudeControl_Multi -#endif - #if FRAME_CONFIG == HELI_FRAME #define MOTOR_CLASS AP_MotorsHeli #else @@ -485,7 +479,8 @@ class Copter : public AP_Vehicle { // Attitude, Position and Waypoint navigation objects // To-Do: move inertial nav up or other navigation variables down here - AC_AttitudeControl_t *attitude_control; + AC_AttitudeControl *attitude_control; + const struct AP_Param::GroupInfo *attitude_control_var_info; AC_PosControl *pos_control; AC_WPNav *wp_nav; AC_Loiter *loiter_nav; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index b449b18e146fc..80a79da030a7c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -498,11 +498,7 @@ const AP_Param::Info Copter::var_info[] = { // @Group: ATC_ // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp -#if FRAME_CONFIG == HELI_FRAME - GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Heli), -#else - GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Multi), -#endif + GOBJECTVARPTR(attitude_control, "ATC_", &copter.attitude_control_var_info), // @Group: PSC // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 3de08e4766a69..fb787077b9059 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -241,7 +241,7 @@ class Mode { AC_PosControl *&pos_control; AP_InertialNav &inertial_nav; AP_AHRS &ahrs; - AC_AttitudeControl_t *&attitude_control; + AC_AttitudeControl *&attitude_control; MOTOR_CLASS *&motors; RC_Channel *&channel_roll; RC_Channel *&channel_pitch; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 6679766a3b41d..4c740a2e3b3e1 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -444,26 +444,24 @@ void Copter::allocate_motors(void) AP_BoardConfig::allocation_error("AP_AHRS_View"); } - const struct AP_Param::GroupInfo *ac_var_info; - #if FRAME_CONFIG != HELI_FRAME if ((AP_Motors::motor_frame_class)g2.frame_class.get() == AP_Motors::MOTOR_FRAME_6DOF_SCRIPTING) { #if AP_SCRIPTING_ENABLED attitude_control = new AC_AttitudeControl_Multi_6DoF(*ahrs_view, aparm, *motors); - ac_var_info = AC_AttitudeControl_Multi_6DoF::var_info; + attitude_control_var_info = AC_AttitudeControl_Multi_6DoF::var_info; #endif // AP_SCRIPTING_ENABLED } else { attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors); - ac_var_info = AC_AttitudeControl_Multi::var_info; + attitude_control_var_info = AC_AttitudeControl_Multi::var_info; } #else attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors); - ac_var_info = AC_AttitudeControl_Heli::var_info; + attitude_control_var_info = AC_AttitudeControl_Heli::var_info; #endif if (attitude_control == nullptr) { AP_BoardConfig::allocation_error("AttitudeControl"); } - AP_Param::load_object_from_eeprom(attitude_control, ac_var_info); + AP_Param::load_object_from_eeprom(attitude_control, attitude_control_var_info); pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); if (pos_control == nullptr) { diff --git a/libraries/AC_CustomControl/AC_CustomControl.cpp b/libraries/AC_CustomControl/AC_CustomControl.cpp index ada094b691e53..a5df09f11b8e8 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl.cpp @@ -38,7 +38,7 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = { const struct AP_Param::GroupInfo *AC_CustomControl::_backend_var_info[CUSTOMCONTROL_MAX_TYPES]; -AC_CustomControl::AC_CustomControl(AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) : +AC_CustomControl::AC_CustomControl(AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) : _dt(dt), _ahrs(ahrs), _att_control(att_control), diff --git a/libraries/AC_CustomControl/AC_CustomControl.h b/libraries/AC_CustomControl/AC_CustomControl.h index 9cda1f21db6cb..e4405b3c28a77 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.h +++ b/libraries/AC_CustomControl/AC_CustomControl.h @@ -6,7 +6,7 @@ #include #include #include -#include +#include #include #include @@ -20,7 +20,7 @@ class AC_CustomControl_Backend; class AC_CustomControl { public: - AC_CustomControl(AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& _att_control, AP_MotorsMulticopter*& motors, float dt); + AC_CustomControl(AP_AHRS_View*& ahrs, AC_AttitudeControl*& _att_control, AP_MotorsMulticopter*& motors, float dt); CLASS_NO_COPY(AC_CustomControl); /* Do not allow copies */ @@ -62,7 +62,7 @@ class AC_CustomControl { // References to external libraries AP_AHRS_View*& _ahrs; - AC_AttitudeControl_Multi*& _att_control; + AC_AttitudeControl*& _att_control; AP_MotorsMulticopter*& _motors; AP_Enum _controller_type; diff --git a/libraries/AC_CustomControl/AC_CustomControl_Backend.h b/libraries/AC_CustomControl/AC_CustomControl_Backend.h index c4068a7b1b8f4..5355ef9cbc18d 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Backend.h +++ b/libraries/AC_CustomControl/AC_CustomControl_Backend.h @@ -7,7 +7,7 @@ class AC_CustomControl_Backend { public: - AC_CustomControl_Backend(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) : + AC_CustomControl_Backend(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) : _frontend(frontend), _ahrs(ahrs), _att_control(att_control), @@ -29,7 +29,7 @@ class AC_CustomControl_Backend protected: // References to external libraries AP_AHRS_View*& _ahrs; - AC_AttitudeControl_Multi*& _att_control; + AC_AttitudeControl*& _att_control; AP_MotorsMulticopter*& _motors; AC_CustomControl& _frontend; }; diff --git a/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp b/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp index 222510c4f471d..5c0550b0898d9 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp @@ -28,7 +28,7 @@ const AP_Param::GroupInfo AC_CustomControl_Empty::var_info[] = { }; // initialize in the constructor -AC_CustomControl_Empty::AC_CustomControl_Empty(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) : +AC_CustomControl_Empty::AC_CustomControl_Empty(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) : AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt) { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AC_CustomControl/AC_CustomControl_Empty.h b/libraries/AC_CustomControl/AC_CustomControl_Empty.h index 692815f1cc520..e3a6374d21624 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Empty.h +++ b/libraries/AC_CustomControl/AC_CustomControl_Empty.h @@ -10,7 +10,7 @@ class AC_CustomControl_Empty : public AC_CustomControl_Backend { public: - AC_CustomControl_Empty(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt); + AC_CustomControl_Empty(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt); Vector3f update(void) override; diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp index ddf6a40c6b4e0..5535d44990940 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp @@ -1,4 +1,5 @@ #include "AC_CustomControl_PID.h" +#include "AC_AttitudeControl/AC_AttitudeControl_Multi.h" #if CUSTOMCONTROL_PID_ENABLED @@ -315,7 +316,7 @@ const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = { AP_GROUPEND }; -AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) : +AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) : AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt), _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.h b/libraries/AC_CustomControl/AC_CustomControl_PID.h index b3cdf68884475..75649a5dd92bf 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_PID.h +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.h @@ -15,7 +15,7 @@ class AC_CustomControl_PID : public AC_CustomControl_Backend { public: - AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt); + AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt); // run lowest level body-frame rate controller and send outputs to the motors Vector3f update() override;