diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 921a6a3e49..5bee84d23c 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -9,13 +9,16 @@ class AP_PitchController { public: - AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash) : - aparm(parms), - autotune(gains, AP_AutoTune::AUTOTUNE_PITCH, parms, _dataflash), - _ahrs(ahrs) - { - AP_Param::setup_object_defaults(this, var_info); - } + static AP_PitchController create(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, + DataFlash_Class &_dataflash) { + return AP_PitchController{ahrs, parms, _dataflash}; + } + + constexpr AP_PitchController(AP_PitchController &&other) = default; + + /* Do not allow copies */ + AP_PitchController(const AP_PitchController &other) = delete; + AP_PitchController &operator=(const AP_PitchController&) = delete; int32_t get_rate_out(float desired_rate, float scaler); int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); @@ -33,9 +36,17 @@ public: AP_Float &kI(void) { return gains.I; } AP_Float &kD(void) { return gains.D; } AP_Float &kFF(void) { return gains.FF; } - + private: - const AP_Vehicle::FixedWing &aparm; + AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash) + : aparm(parms) + , autotune(gains, AP_AutoTune::AUTOTUNE_PITCH, parms, _dataflash) + , _ahrs(ahrs) + { + AP_Param::setup_object_defaults(this, var_info); + } + + const AP_Vehicle::FixedWing &aparm; AP_AutoTune::ATGains gains; AP_AutoTune autotune; AP_Int16 _max_rate_neg; diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index a93926c5c9..e38057734d 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -9,13 +9,16 @@ class AP_RollController { public: - AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash) : - aparm(parms), - autotune(gains, AP_AutoTune::AUTOTUNE_ROLL, parms, _dataflash), - _ahrs(ahrs) - { - AP_Param::setup_object_defaults(this, var_info); - } + static AP_RollController create(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, + DataFlash_Class &_dataflash) { + return AP_RollController{ahrs, parms, _dataflash}; + } + + constexpr AP_RollController(AP_RollController &&other) = default; + + /* Do not allow copies */ + AP_RollController(const AP_RollController &other) = delete; + AP_RollController &operator=(const AP_RollController&) = delete; int32_t get_rate_out(float desired_rate, float scaler); int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); @@ -40,9 +43,17 @@ public: AP_Float &kI(void) { return gains.I; } AP_Float &kD(void) { return gains.D; } AP_Float &kFF(void) { return gains.FF; } - + private: - const AP_Vehicle::FixedWing &aparm; + AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash) + : aparm(parms) + , autotune(gains, AP_AutoTune::AUTOTUNE_ROLL, parms, _dataflash) + , _ahrs(ahrs) + { + AP_Param::setup_object_defaults(this, var_info); + } + + const AP_Vehicle::FixedWing &aparm; AP_AutoTune::ATGains gains; AP_AutoTune autotune; uint32_t _last_t; diff --git a/libraries/APM_Control/AP_SteerController.h b/libraries/APM_Control/AP_SteerController.h index 93a85a8d43..49b0629670 100644 --- a/libraries/APM_Control/AP_SteerController.h +++ b/libraries/APM_Control/AP_SteerController.h @@ -7,11 +7,15 @@ class AP_SteerController { public: - AP_SteerController(AP_AHRS &ahrs) : - _ahrs(ahrs) - { - AP_Param::setup_object_defaults(this, var_info); - } + static AP_SteerController create(AP_AHRS &ahrs) { + return AP_SteerController{ahrs}; + } + + constexpr AP_SteerController(AP_SteerController &&other) = default; + + /* Do not allow copies */ + AP_SteerController(const AP_SteerController &other) = delete; + AP_SteerController &operator=(const AP_SteerController&) = delete; /* return a steering servo output from -4500 to 4500 given a @@ -49,7 +53,13 @@ public: } private: - AP_Float _tau; + AP_SteerController(AP_AHRS &ahrs) + : _ahrs(ahrs) + { + AP_Param::setup_object_defaults(this, var_info); + } + + AP_Float _tau; AP_Float _K_FF; AP_Float _K_P; AP_Float _K_I; diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index fe25dbd832..3326f61206 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -7,16 +7,16 @@ #include class AP_YawController { -public: - AP_YawController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) : - aparm(parms), - _ahrs(ahrs) - { - AP_Param::setup_object_defaults(this, var_info); - _pid_info.desired = 0; - _pid_info.FF = 0; - _pid_info.P = 0; - } +public: + static AP_YawController create(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) { + return AP_YawController{ahrs, parms}; + } + + constexpr AP_YawController(AP_YawController &&other) = default; + + /* Do not allow copies */ + AP_YawController(const AP_YawController &other) = delete; + AP_YawController &operator=(const AP_YawController&) = delete; int32_t get_servo_out(float scaler, bool disable_integrator); @@ -27,7 +27,17 @@ public: static const struct AP_Param::GroupInfo var_info[]; private: - const AP_Vehicle::FixedWing &aparm; + AP_YawController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) + : aparm(parms) + , _ahrs(ahrs) + { + AP_Param::setup_object_defaults(this, var_info); + _pid_info.desired = 0; + _pid_info.FF = 0; + _pid_info.P = 0; + } + + const AP_Vehicle::FixedWing &aparm; AP_Float _K_A; AP_Float _K_I; AP_Float _K_D;