2022-08-04 21:18:55 -03:00
|
|
|
#pragma once
|
|
|
|
|
2024-02-14 20:40:28 -04:00
|
|
|
#include "AC_CustomControl_config.h"
|
2022-08-04 21:18:55 -03:00
|
|
|
|
|
|
|
#if AP_CUSTOMCONTROL_ENABLED
|
|
|
|
|
2024-02-14 20:40:28 -04:00
|
|
|
#include "AC_CustomControl.h"
|
|
|
|
|
2022-08-04 21:18:55 -03:00
|
|
|
class AC_CustomControl_Backend
|
|
|
|
{
|
|
|
|
public:
|
2023-07-19 10:08:19 -03:00
|
|
|
AC_CustomControl_Backend(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) :
|
2022-08-04 21:18:55 -03:00
|
|
|
_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;
|
|
|
|
|
2023-08-10 08:09:49 -03:00
|
|
|
// set the PID notch sample rates
|
|
|
|
virtual void set_notch_sample_rate(float sample_rate) {};
|
|
|
|
|
2022-08-04 21:18:55 -03:00
|
|
|
protected:
|
|
|
|
// References to external libraries
|
|
|
|
AP_AHRS_View*& _ahrs;
|
2023-07-19 10:08:19 -03:00
|
|
|
AC_AttitudeControl*& _att_control;
|
2022-08-04 21:18:55 -03:00
|
|
|
AP_MotorsMulticopter*& _motors;
|
|
|
|
AC_CustomControl& _frontend;
|
|
|
|
};
|
|
|
|
|
2024-02-14 20:40:28 -04:00
|
|
|
#endif // AP_CUSTOMCONTROL_ENABLED
|