diff --git a/libraries/AC_CustomControl/AC_CustomControl.cpp b/libraries/AC_CustomControl/AC_CustomControl.cpp index b86647cb37..c0b4625491 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl.cpp @@ -6,13 +6,14 @@ #include "AC_CustomControl_Backend.h" +// #include "AC_CustomControl_Empty.h" // table of user settable parameters const AP_Param::GroupInfo AC_CustomControl::var_info[] = { // @Param: _TYPE // @DisplayName: Custom control type // @Description: Custom control type to be used - // @Values: 0:None + // @Values: 0:None, 1:Empty // @RebootRequired: True // @User: Advanced AP_GROUPINFO_FLAGS("_TYPE", 1, AC_CustomControl, _controller_type, 0, AP_PARAM_FLAG_ENABLE), @@ -24,6 +25,9 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = { // @User: Advanced AP_GROUPINFO("_AXIS_MASK", 2, AC_CustomControl, _custom_controller_mask, 0), + // parameters for empty controller. only used as a template, no need for param table + // AP_SUBGROUPVARPTR(_backend, "1_", 6, AC_CustomControl, _backend_var_info[0]), + AP_GROUPEND }; @@ -44,6 +48,11 @@ void AC_CustomControl::init(void) { case CustomControlType::CONT_NONE: break; + case CustomControlType::CONT_EMPTY: // This is template backend. Don't initialize it. + // This is template backend. Don't initialize it. + // _backend = new AC_CustomControl_Empty(*this, _ahrs, _att_control, _motors, _dt); + // _backend_var_info[get_type()] = AC_CustomControl_Empty::var_info; + break; default: return; } diff --git a/libraries/AC_CustomControl/AC_CustomControl.h b/libraries/AC_CustomControl/AC_CustomControl.h index dfb3097993..3e92df5684 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.h +++ b/libraries/AC_CustomControl/AC_CustomControl.h @@ -43,6 +43,7 @@ protected: // add custom controller here enum class CustomControlType : uint8_t { CONT_NONE = 0, + CONT_EMPTY = 1, }; // controller that should be used enum class CustomControlOption { diff --git a/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp b/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp new file mode 100644 index 0000000000..d11caabc7e --- /dev/null +++ b/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp @@ -0,0 +1,72 @@ +#include "AC_CustomControl_Empty.h" + +#if CUSTOMCONTROL_EMPTY_ENABLED + +#include + +// table of user settable parameters +const AP_Param::GroupInfo AC_CustomControl_Empty::var_info[] = { + // @Param: PARAM1 + // @DisplayName: Empty param1 + // @Description: Dumy parameter for empty custom controller backend + // @User: Advanced + AP_GROUPINFO("PARAM1", 1, AC_CustomControl_Empty, param1, 0.0f), + + // @Param: PARAM2 + // @DisplayName: Empty param2 + // @Description: Dumy parameter for empty custom controller backend + // @User: Advanced + AP_GROUPINFO("PARAM2", 2, AC_CustomControl_Empty, param2, 0.0f), + + // @Param: PARAM3 + // @DisplayName: Empty param3 + // @Description: Dumy parameter for empty custom controller backend + // @User: Advanced + AP_GROUPINFO("PARAM3", 3, AC_CustomControl_Empty, param3, 0.0f), + + AP_GROUPEND +}; + +// 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_Backend(frontend, ahrs, att_control, motors, dt) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +// update controller +// return roll, pitch, yaw controller output +Vector3f AC_CustomControl_Empty::update(void) +{ + // reset controller based on spool state + switch (_motors->get_spool_state()) { + case AP_Motors::SpoolState::SHUT_DOWN: + case AP_Motors::SpoolState::GROUND_IDLE: + // We are still at the ground. Reset custom controller to avoid + // build up, ex: integrator + reset(); + break; + + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: + case AP_Motors::SpoolState::SPOOLING_UP: + case AP_Motors::SpoolState::SPOOLING_DOWN: + // we are off the ground + break; + } + + // arducopter main attitude controller already runned + // we don't need to do anything else + + 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()); +} + +// reset controller to avoid build up on the ground +// or to provide bumpless transfer from arducopter main controller +void AC_CustomControl_Empty::reset(void) +{ +} + +#endif diff --git a/libraries/AC_CustomControl/AC_CustomControl_Empty.h b/libraries/AC_CustomControl/AC_CustomControl_Empty.h new file mode 100644 index 0000000000..692815f1cc --- /dev/null +++ b/libraries/AC_CustomControl/AC_CustomControl_Empty.h @@ -0,0 +1,29 @@ +#pragma once + +#include "AC_CustomControl_Backend.h" + +#ifndef CUSTOMCONTROL_EMPTY_ENABLED + #define CUSTOMCONTROL_EMPTY_ENABLED AP_CUSTOMCONTROL_ENABLED +#endif + +#if CUSTOMCONTROL_EMPTY_ENABLED + +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); + + + Vector3f update(void) override; + void reset(void) override; + + // user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + +protected: + // declare parameters here + AP_Float param1; + AP_Float param2; + AP_Float param3; +}; + +#endif