AC_CustomControl: add custom controller support

libraries/AC_CustomControl/AC_CustomControl.cpp

@@ -0,0 +1,169 @@
+#include <AP_HAL/AP_HAL.h>
+
+#include "AC_CustomControl.h"
+
+#if AP_CUSTOMCONTROL_ENABLED
+
+
+#include "AC_CustomControl_Backend.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
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("_TYPE", 1, AC_CustomControl, _controller_type, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: _AXIS_MASK
+    // @DisplayName: Custom Controller bitmask
+    // @Description: Custom Controller bitmask to chose which axis to run
+    // @Bitmask: 0:Roll, 1:Pitch, 2:Yaw
+    // @User: Advanced
+    AP_GROUPINFO("_AXIS_MASK", 2, AC_CustomControl, _custom_controller_mask, 0),
+
+    AP_GROUPEND
+};
+
+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) :
+    _dt(dt),
+    _ahrs(ahrs),
+    _att_control(att_control),
+    _motors(motors)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+void AC_CustomControl::init(void)
+{
+    switch (CustomControlType(_controller_type))
+    {
+        case CustomControlType::CONT_NONE:
+            break;
+        default:
+            return;
+    }
+
+    if (_backend && _backend_var_info[get_type()]) {
+        AP_Param::load_object_from_eeprom(_backend, _backend_var_info[get_type()]);
+    }
+}
+
+// run custom controller if it is activated by RC switch and appropriate type is selected
+void AC_CustomControl::update(void)
+{
+    if (is_safe_to_run()) {
+        Vector3f motor_out_rpy;
+
+        motor_out_rpy = _backend->update();
+
+        motor_set(motor_out_rpy);
+    }
+}
+
+// choose which axis to apply custom controller output
+void AC_CustomControl::motor_set(Vector3f rpy) {
+    if (_custom_controller_mask & (uint8_t)CustomControlOption::ROLL) {
+        _motors->set_roll(rpy.x);
+        _att_control->get_rate_roll_pid().set_integrator(0.0);
+    }
+    if (_custom_controller_mask & (uint8_t)CustomControlOption::PITCH) {
+        _motors->set_pitch(rpy.y);
+        _att_control->get_rate_pitch_pid().set_integrator(0.0);
+    }
+    if (_custom_controller_mask & (uint8_t)CustomControlOption::YAW) {
+        _motors->set_yaw(rpy.z);
+        _att_control->get_rate_yaw_pid().set_integrator(0.0);
+    }
+}
+
+// move main controller's target to current states, reset filters,
+// and move integrator to motor output
+// to allow smooth transition to the primary controller
+void AC_CustomControl::reset_main_att_controller(void)
+{
+    // reset attitude and rate target, if feedforward is enabled
+    if (_att_control->get_bf_feedforward()) {
+        _att_control->relax_attitude_controllers();
+    }
+
+    _att_control->get_rate_roll_pid().set_integrator(0.0);
+    _att_control->get_rate_pitch_pid().set_integrator(0.0);
+    _att_control->get_rate_yaw_pid().set_integrator(0.0);
+}
+
+void AC_CustomControl::set_custom_controller(bool enabled)
+{
+    // double logging switch makes the state change very clear in the log
+    log_switch();
+
+    _custom_controller_active = false;
+
+    // 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");
+        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");
+        return;
+    }
+
+    // backend is not created
+    if (_backend == nullptr) {
+        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");
+        return;
+    }
+
+    // reset main controller
+    if (!enabled) {
+        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();
+        }
+    }
+
+    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");
+    }
+
+    _custom_controller_active = enabled;
+
+    // log successful switch
+    log_switch();
+}
+
+// check that RC switch is on, backend is not changed mid flight and controller type is selected
+bool AC_CustomControl::is_safe_to_run(void) {
+    if (_custom_controller_active && (_controller_type > CustomControlType::CONT_NONE)
+        && (_controller_type <= CUSTOMCONTROL_MAX_TYPES) && _backend != nullptr)
+    {
+        return true;
+    }
+
+    return false;
+}
+
+// log when the custom controller is switch into
+void AC_CustomControl::log_switch(void) {
+    AP::logger().Write("CC", "TimeUS,Type,Act","QBB",
+                            AP_HAL::micros64(),
+                            _controller_type,
+                            _custom_controller_active);
+}
+
+#endif

libraries/AC_CustomControl/AC_CustomControl.h

@@ -0,0 +1,70 @@
+#pragma once
+
+/// @file    AC_CustomControl.h
+/// @brief   ArduCopter custom control library
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Param/AP_Param.h>
+#include <AP_AHRS/AP_AHRS_View.h>
+#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h>
+#include <AP_Motors/AP_MotorsMulticopter.h>
+#include <AP_Logger/AP_Logger.h>
+
+#if AP_CUSTOMCONTROL_ENABLED
+
+#ifndef CUSTOMCONTROL_MAX_TYPES
+#define CUSTOMCONTROL_MAX_TYPES 1
+#endif
+
+class AC_CustomControl_Backend;
+
+class AC_CustomControl {
+public:
+    AC_CustomControl(AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& _att_control, AP_MotorsMulticopter*& motors, float dt);
+
+    CLASS_NO_COPY(AC_CustomControl);  /* Do not allow copies */
+
+    void init(void);
+    void update(void);
+    void motor_set(Vector3f motor_out);
+    void set_custom_controller(bool enabled);
+    void reset_main_att_controller(void);
+    bool is_safe_to_run(void);
+    void log_switch(void);
+
+    // zero index controller type param, only use it to acces _backend or _backend_var_info array
+    uint8_t get_type() { return _controller_type > 0 ? (_controller_type - 1) : 0; };
+
+    // User settable parameters
+    static const struct AP_Param::GroupInfo var_info[];
+    static const struct AP_Param::GroupInfo *_backend_var_info[CUSTOMCONTROL_MAX_TYPES];
+
+protected:
+    // add custom controller here
+    enum class CustomControlType : uint8_t {
+        CONT_NONE            = 0,
+    };            // controller that should be used     
+
+    enum class  CustomControlOption {
+        ROLL = 1 << 0,
+        PITCH = 1 << 1,
+        YAW = 1 << 2,
+    };
+
+    // Intersampling period in seconds
+    float _dt;
+    bool _custom_controller_active;
+
+    // References to external libraries
+    AP_AHRS_View*& _ahrs;
+    AC_AttitudeControl_Multi*& _att_control;
+    AP_MotorsMulticopter*& _motors;
+
+    AP_Enum<CustomControlType> _controller_type;
+    AP_Int8 _custom_controller_mask;
+
+private:
+    AC_CustomControl_Backend *_backend;
+};
+
+#endif

libraries/AC_CustomControl/AC_CustomControl_Backend.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "AC_CustomControl.h"
+
+#if AP_CUSTOMCONTROL_ENABLED
+
+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) :
+        _frontend(frontend),
+        _ahrs(ahrs),
+        _att_control(att_control),
+        _motors(motors)
+    {}
+
+    // empty destructor to suppress compiler warning
+    virtual ~AC_CustomControl_Backend() {}
+
+    // update controller, return roll, pitch, yaw controller output
+    virtual Vector3f update() = 0;
+
+    // reset controller to avoid build up or abrupt response upon switch, ex: integrator, filter
+    virtual void reset() = 0;
+
+protected:
+    // References to external libraries
+    AP_AHRS_View*& _ahrs;
+    AC_AttitudeControl_Multi*& _att_control;
+    AP_MotorsMulticopter*& _motors;
+    AC_CustomControl& _frontend;
+};
+
+#endif